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FOREWORD 

The  classical  problem  of  weapons  control  involves  predicting  the  future  position  of  a 
maneuvering  target.  Critical  to  successful  prediction  is  the  accurate  estimation  of  the  current 
target  state.  With  the  advent  of  guided  weapons,  the  consequences  of  threat  maneuver  are 
reduced  when  accurate  estimates  of  the  target  state  can  be  obtained.  Threat  trends  indicate 
that  the  conditions  under  which  hostile  targets  can  be  engaged  successfully  are  becoming 
more  difficult  to  achieve;  hence,  any  improvement  in  existing  estimation  algorithms  is  of 
critical  importance. 

This  report  presents  the  results  of  an  investigation  of  an  approach  to  the  improvement 
of  an  important  class  of  target  tracking  algorithms.  The  work  was  supported  by  the  Naval 
Surface  Warfare  Center  Dahlgren  Division  (NSWCDD)  AEGIS  Program  Office. 

This  document  has  been  reviewed  by  Dr.  Richard  D.  Hilton  of  the  Command  Support 
Systems  Division  and  R.  T.  Lee,  Head,  Weapons  Control  Division. 


Approved  by: 


DAVID  S.  MALYEVAC,  Deputy  Department  Head 
Weapons  Systems  Department 
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ABSTRACT 

The  two-stage  Alpha-Beta-Gamma  estimator  is  proposed  as  an  alternative  to  adaptive 
gain  versions  of  the  Alpha-Beta  and  Alpha-Beta-Gamma  filters  for  tracking  maneuvering 
targets.  The  purpose  of  this  report  is  to  accomplish  fixed-gain,  variable  dimension  filtering 
with  a  two-stage  Alpha-Beta-Gamma  estimator.  The  two-stage  Alpha-Beta-Gamma  estima¬ 
tor  is  derived  from  the  two-stage  Kalman  estimator,  and  the  noise  variance  reduction  matrix 
and  steady-state  error  covariance  matrix  are  given  as  a  function  of  the  steady-state  gains. 
A  procedure  for  filter  parameter  selection  is  also  given  along  with  a  technique  for  maneuver 
response  and  a  gain  scheduling  technique  for  initialization.  The  kinematic  constraint  for 
constant  speed  targets  is  also  incorporated  into  the  two-stage  estimator  to  form  the  two- 
stage  Alpha-Beta-Gamma-Lambda  estimator.  Simulation  results  are  given  for  a  comparison 
of  the  performances  of  estimators  with  that  of  the  Alpha-Beta-Gamma  filter. 
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CHAPTER  1 
INTRODUCTION 


While  the  Kalman  filter  is  known  to  produce  an  optimal  estimate  of  the  target  state 
when  given  the  motion  model  and  a  sequence  of  sensor  measurements  corrupted  with  white 
Gaussian  errors,  the  computational  burden  of  maintaining  the  Kalman  filter  may  prohibit 
its  use  when  many  targets  are  being  tracked.  A  widely  used  approach  to  reducing  the 
computational  burden  of  the  filters  involves  the  use  of  an  approximate  filter  gain  instead  of 
the  optimal  Kalman  gain.  The  gain  is  usually  based  on  the  steady-state  gains  of  the  Kalman 
filter  and  obtained  by  either  using  a  fixed  gain  or  an  easily  generated  gain  schedule.  When 
these  steady-state  or  approximate  gains  are  used,  the  resultant  filters  are  called  an  a  filter 
for  tracking  position;  an  a, 0  filter  for  tracking  position  and  velocity;  and  an  a,/?, 7  filter 
tracking  position,  velocity,  and  acceleration. 

For  tracking  systems  with  a  uniform  data  rate  and  stationary  measurement  noise,  non¬ 
maneuvering  targets  can  be  accurately  tracked  with  an  a,  0  filter.  However,  when  the  target 
maneuvers,  the  quality  of  the  position  and  velocity  estimates  provided  by  the  filter  can  de¬ 
grade  significantly,  and  for  a  target  undergoing  a  large  maneuver,  the  target  track  may  be 
lost.  An  a,  /?,  7  filter  can  be  used  to  track  such  a  target,  but  the  accelerations  of  a  maneuver¬ 
ing  target  are  seldom  constant  in  the  tracking  frame  during  the  maneuver.  If  the  a  gain  of 
the  a,  0, 7  filter  is  chosen  small  to  achieve  good  noise  reduction,  the  “optimal”  7  of  Kalata  [1] 
will  be  very  small  and  the  acceleration  estimates  will  respond  slowly  to  a  maneuver.  Thus, 
in  order  for  the  a,  0, 7  filter  to  respond  quickly  to  a  maneuver,  a  must  be  maintained  at  a 
high  level  and  the  filter  will  provide  poor  noise  reduction  (noisy  estimates)  when  the  target 
is  not  manuevering.  The  noisy  target  state  estimates  may  hinder  the  sensor  pointing,  data 
association,  and  other  control  decisions  and  computations  based  on  the  state  estimates. 

One  approach  to  overcoming  this  filtering  dilemma  is  to  use  a  small  a  until  a  maneuver 
is  detected.  Then  a  is  increased  for  tracking  through  the  maneuver.  This  approach  was 
investigated  by  Cantrell  [2]  and  Blackman  [3].  It  has  two  major  problems.  First,  the  response 
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of  the  filter  is  significantly  delayed  because  the  gains  are  not  increased  until  a  maneuver  is 
detected.  Furthermore,  in  the  a,  0  filter,  the  a  gain  must  be  set  artificially  high  to  account 
for  the  absence  of  acceleration  from  the  motion  model.  Second,  after  a  target  has  returned 
to  constant  velocity  from  a  maneuver,  the  decision  to  reduce  a  is  often  delayed  because  there 
is  no  bias  in  the  higher  gain  a,/?  filter  to  detect. 

An  alternate  approach,  which  seems  to  be  attractive,  is  to  continuously  estimate  the 
target  acceleration  and  correct  the  position  and  velocity  estimates  of  the  er,  0  filter  whenever 
a  maneuver  is  detected.  The  purpose  of  this  report  is  to  accomplish  fixed-gain,  variable 
dimension  filtering  with  a  two-stage  a,  0, 7  estimator,  as  suggested  by  Alouani  et.  al.  [4,5] 
for  the  two-stage  Kalman  estimator.  In  the  two-stage  ct,/?,  7  estimator,  position  and  velocity 
are  tracked  in  the  first  stage,  which  is  a  standard  a,  0  filter,  and  the  acceleration  is  tracked  in 
the  second  stage,  which  is  a  standard  single  gain  filter.  The  output  of  the  acceleration  filter 
can  be  used  to  adjust  the  estimates  of  the  a, /?  filter,  as  shown  in  Figure  1-1  when  a  bias  is 
detected.  This  approach  to  tracking  maneuvering  targets  is  similar  to  input  estimation  in 
[6-8],  with  the  exception  that  the  acceleration  is  modeled  as  a  stochastic  process  instead  of 
a  deterministic  process  as  in  input  estimation.  The  two-stage  estimator  has  the  good  noise 
reduction  associated  with  the  a,0  filter  when  the  target  is  not  maneuvering,  and  when  a 
maneuver  is  detected,  an  acceleration  estimate  is  available  to  compensate  the  estimates  of 
the  a,  0  filter.  Furthermore,  since  the  acceleration  is  not  part  of  the  central  filter,  the  7  gain 
can  be  picked  somewhat  independent  of  a  to  improve  maneuver  response. 

When  targets  maneuver  while  maintaining  a  constant  speed,  the  accelerations  vary  with 
time  throughout  the  maneuver  and  the  estimates  of  both  the  a,  0, 7  filter  and  the  two-stage 
a, /?,  7  estimator  will  be  biased.  The  kinematic  constraint  for  constant  speed  targets  can 
be  included  as  a  pseudomeasurement  in  the  filtering  process  to  reduce  the  filter  bias  as  in 
[9].  The  kinematic  constraint  for  constant  speed  targets  is  incorporated  into  the  two-stage 
estimator  to  form  the  two-stage  a,  (3, 7,  A  estimator. 

This  report  is  organized  as  follows.  In  Chapter  2,  steady-state  Kalman  filters  are  dis¬ 
cussed,  and  the  a,  (3  filter  and  o,  f3, 7  filter  are  summarized.  The  two-stage  Kalman  estimator 
is  presented  in  Chapter  3.  In  Chapter  4,  the  two-stage  a,  0, 7  estimator  is  derived,  and  proce¬ 
dures  for  gain  selection  during  initialization  and  steady-state  conditions  are  presented  along 
with  the  concept  of  a  soft  switch  for  maneuver  response.  The  two-stage  a,  0, 7,  A  estimator 
is  presented  in  Chapter  5.  Simulation  results  for  a  radar  tracking  system  are  presented  in 
Chapter  6  for  the  two-stage  estimators  and  the  a,0, 7  filter.  Some  concluding  remarks  are 
given  in  Chapter  7  along  with  a  discussion  of  future  research. 
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Figure  1-1.  Two-Stage  Estimator  for  Tracking  Maneuvering  Targets 
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CHAPTER  2 

STEADY-STATE  KALMAN  FILTERS 

A  Kalman  filter  is  often  employed  to  filter  the  position  measurements  for  estimating  the 
position,  velocity,  and/or  acceleration  of  a  target.  When  the  target  motion  and  measurement 
models  are  linear  and  the  measurement  and  motion  modeling  error  processes  are  Gaussian, 
the  Kalman  filter  provides  the  minimum  mean-square  error  estimate  of  the  target  state. 
When  the  target  motion  and  measurement  models  are  linear,  but  the  noise  processes  are  not 
Gaussian,  the  Kalman  filter  is  the  best  linear  estimator  of  the  target  state  in  the  mean-square 
error  sense.  The  dynamics  model  commonly  assumed  for  a  target  in  track  is  given  by 

Xk+ 1  =  FjfcXjt  4-  GkW k  (2.1) 

where  ~  N( 0,  Qk)  is  the  process  noise  and  Fk  defines  a  linear  constraint  on  the  dynamics. 
The  target  state  vector  A*  contains  the  position,  velocity,  and  acceleration  of  the  target  at 
time  k,  as  well  as  other  variables  used  to  model  the  time-varying  acceleration.  The  linear 
measurement  model  is  given  by 

Yk  =  HkXk  +  nk  (2.2) 

where  Yk  is  usually  the  target  position  measurement  and  nk  ~  N( 0,  Rk).  The  Kalman 
filtering  equations  associated  with  the  state  model  in  Eq.  (2.1)  and  the  measurement  model 
in  Eq.  (2.2)  are  given  by  the  following  equations. 


Time  Update: 

Ajt|*-i  =  Fk-\Xk_i\k~i  (2.3) 

P*|Jt-i  =  +  Gfc-iQfc-iGjf-i  (2-4) 

Measurement  Update: 

Kt  =  (2.5) 

+  Kt{Yt  -  (2.6) 

Pt\k  =  II  -  KkHk)Pt |*_i  (2.7) 
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where  X*  ~  N(Xk\k,  Pk\k)  with  %k\k  and  Pk\k  denoting  the  mean  and  error  covariance  of  the 
state  estimate,  respectively.  The  subscript  notation  ( k\j )  denotes  the  state  estimate  for  time 
k  when  given  measurements  through  time  j,  and  A*  denotes  the  Kalman  gain.  Using  the 
matrix  inversion  lemma  of  [10]  and  Eqs.  (2.5)  and  (2.7),  an  alternate  form  of  the  Kalman 
gain  is  given  by 

Kt  =  +  «r,r' 

=  11- 

=  [/  -  Pk\t-lHkUhPt\k-,Hl  +  K-t'r'Ht\Pk\t-,HtRl' 

=  PllkH{R^  (2.8) 

The  steady-state  form  of  the  Kalman  filter  is  often  used  in  order  to  reduce  the  computa¬ 
tions  required  to  maintain  each  track.  In  steady-state,  and  Pk+\]k  — 

and  Kk  =  /C Jt — 1  •  F°r  a  Kalman  filter  to  achieve  these  steady-state  conditions,  the  error  pro¬ 
cesses,  wk  and  nfc,  must  have  stationary  statistics  and  the  data  rate  must  be  constant. 
When  the  noise  processes  are  not  stationary  or  the  data  rate  is  not  constant,  a  filter  using 
the  steady-state  gains  will  provide  suboptimal  estimates.  The  a,/?  and  a,/3,7  filters  are  the 
steady-state  Kalman  filters  for  tracking  nearly  constant  velocity  targets  and  nearly  constant 
acceleration  targets,  respectively.  First,  the  a  filter  for  tracking  nearly  constant  position 
targets  will  be  considered.  Then  the  a,  (3  and  a,/?, 7  filters  will  be  considered. 

ALPHA  FILTER 

The  a  filter  is  a  single  coordinate  filter  that  is  based  on  the  assumption  that  the  tar¬ 
get  position  is  constant  plus  zero-mean,  white  Gaussian  acceleration  errors.  Given  this 
assumption,  the  filter  gain  a  is  chosen  as  the  steady-state  Kalman  gain  that  minimize  the 
mean-square  error  in  the  position  estimates.  For  the  a  filter, 


A’t  =  [xl]T 

(2.9) 

n  =  [i] 

(2.10) 

Gk  = 

'  T2  ' 
.  2  . 

T 

(2.11) 

#*  =  [  1] 

(2.12) 

Rk  =  *1 

(2.13) 

Qk  =  *1 

(2.14) 
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*i  =  [a]T 


(2.15) 


The  steady-state  error  covariance  of  the  filtered  estimates  for  the  a  filter  can  be  expressed 
as  a  function  the  Kalman  gain  using  Eq.  (2.8)  to  be 

JVn|*+i  =  Pk\k  =  P  =  <*oi  (2.16) 


Using  Eq.  (2.4)  in  Eq.  (2.7)  and  inserting  Eq.  (2.16)  for  P  gives 

cxa\  =  (1  -  a}[a<jl  +  (2.17) 

Thus,  Eq.  (2.17)  gives 


p2  _  ji4  Ow 


Aal 
1  —  a 


(2.18) 


where  T  is  the  Tracking  Index  of  [11],  and  is  the  variance  of  the  acceleration  modeling 
error.  Thus,  the  a  gain  is  determined  as  in  (1]  from  T  by  solving  the  Eq.  (2.18).  Note  the 
these  steady-state  relationships  result  from  the  assumption  that  the  model  error  process  is 
acceleration  errors  that  are  constant  through  each  measurement  sample  period. 


The  input-output  relationships  between  the  measurements  V*  and  can  be  expressed 
as  a  linear  system  that  is  given  by 

Xk\k  =  (l-a  )**_,)*_! +«n  (2.19) 

Using  Eq.  (2  19)  provides  the  error  covariance  of  that  results  from  the  measurement 
errors.  Let  V*  be  a  white  noise  stationary  sequence  with  zero  mean.  Then  E[XkXk]  =  5*  = 
5 *_i  =  5a,  which  is  given  by 

—  (T^Q 

=  (2.20) 

l  —  Q 

The  variance  reduction  ratio  of  the  filter  is  given  by  Eq.  (2.20)  when  =  1. 

Since  during  initialization  the  a  filter  is  not  in  a  steady-state  condition,  Eq.  (2.16)  cannot 
be  used  to  determine  a.  Using  least-squares  estimation  in  conjunction  with  a  constant  state 
model,  a  simple  gain  scheduling  procedure  for  a  during  initialization  is  given  by 

ak  =  max{-|--,a}  (2.21) 

ji 

with  Ao|_i  =  [0]  .  The  derivation  of  Eq.  (2  21)  follows  closely  the  derivation  for  the  gains 
for  initializing  the  a,  0  filter  in  Appendix  A. 
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ALPHA-BETA  FILTER 


The  a,0  filter  is  a  single  coordinate  filter  that  is  baaed  on  the  assumption  that  the  target 
is  moving  with  constant  velocity  plus  zero-mean,  white  Gaussian  acceleration  errors.  Given 
this  assumption,  the  filter  gains  a  and  0  are  chosen  as  the  steady-state  Kalman  gains  that 
minimize  the  mean-square  error  in  the  position  and  velocity  estimates.  For  the  a,  0  filter, 


AT 

Fh 

Gk 

Hk 

Rk 

Qk 

Kk 


[**  ikf 

1  T 
0  1 


T 


T 


[1  0] 


The  a,0  gains  are  determined  as  in  [1,3,8]  by  solving  the  simultaneous  equations 


r2 


-.4  <rw 


(1  -«) 


0  =  2(2  -  a)  ~  4v/f^  a 


(2.22) 

(2.23) 

(2.24) 

(2.25) 

(2.2.') 

(2.27) 

(2.28) 


(2.29) 

(2.30) 


where  a2w  is  ...e  variance  of  the  acceleration  modeling  error. 


The  steady-state  error  covariance  of  the  filtered  estimates  for  the  a,  0  filter  is  given  in 
[1 ,8]  as 

0 


Fk\k  =  < 


a 


0  0(2 a  -  0) 

T  2(1  -a)T2 


(2.31) 


The  steady-state  gains  and  error  covariance  in  Eqs.  (2.29)  through  (2.31)  are  derived  in 
Appendix  A.  Note  that  these  steady-state  relationships  result  from  the  assumption  that 
the  model  error  process  is  acceleration  errors  that  are  constant  through  each  measurement 
sample  period. 


In  target  tracking,  a  conflict  arises  between  the  goals  of  good  noise  reduction  which 
requires  small  a  (and  thus,  small  0),  and  good  tracking  through  maneuvers  which  requires 
a  larger  a.  For  good  noise  reduction,  heavy  filtering  is  used  and  a  filter  with  slow  system 
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response  (i.e.,  a  large  time  constant)  and  a  narrow  bandwidth  results.  For  good  tracking 
through  a  maneuver,  light  filtering  is  performed  and  a  filter  with  fast  response  (i.e.,  a  short 
time  constant)  and  a  wide  bandwidth  results.  The  a  and  ft  are  chosen  with  the  “optimal” 
relationship  in  Eq.  (2.29)  to  obtain  a  compromise  between  these  two  goals  that  meets  the 
design  requirements. 

The  input-output  relationships  between  the  measurements  V*  and  -X*|*  can  be  expressed 
as  a  linear  system  that  is  given  by 

^*1*  =  +  GkYk 

where 


a 

T 

Using  Eq.  (2.32)  provides  the  error  covariance  of  that  results  from  the  measurement 
errors.  That  error  covariance  5*  is  given  by 

Sk  =  F  Sk-iF7  +  G  GTa2v  (2.35) 

Since  S’*  =  S*_i  =  Sap  in  steady-state  conditions,  Eq.  (2.35)  can  be  used  to  solve  for  Sap 
in  terms  of  the  filter  gains,  measurement  period,  and  the  measurement  error  variance.  As 
derived  in  Appendix  A, 

_  _  1 2a2  +  0(2  -  3a)  |(2a  -  0) 

Sa>  =  l,. 

where  d\  ~  4  —  2a  —  /?.  The  variance  reduction  ratios  of  the  filter  are  given  by  Eq.  (2.36)  when 
ex2  =  1  with  the  (1,1)  and  (2,2)  elements  of  SQp  denoting  the  position  variance  reduction 
ratio  and  velocity  variance  reduction  ratio,  respectively  [3]. 

Since  during  initialization  the  a,  0  filter  is  not  in  steady-state  conditions,  Eqs.  (2.29)  and 
(2.30)  cannot  be  used  to  determine  a  and  f3.  Using  least-squares  estimation  in  conjunction 
with  a  constant  velocity  motion  model,  a  simple  gain  scheduling  procedure  for  a  and  0 


(2.36) 


(2.32) 
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during  initialization  is  given  by 


,  2(2* +  1) 

a‘-max{(t+l)(fc  +  2)'“) 

(2.37) 

A_max{(t+i)(t  +  2),/3} 

(2.38) 

rp 

with  Xo|_j  =  [0  0  j  .  Note  that  if  Yq/T  can  be  extremely  large  relative  to  the  dynamic 

range  of  the  compute*,  the  first  two  filter  iterations  should  be  computed  analytically  to  use 
a  two-point  initialization  procedure. 

ALPHA-BETA-GAMMA  FILTER 

The  a,  0, 7  filter  is  a  single  coordinate  filter  that  is  based  on  the  assumption  that  the  tar¬ 
get  is  moving  with  constant  acceleration  plus  zero- mean,  white  Gaussian  acceleration  errors. 
Given  this  assumption,  the  a,0, 7  filter  gains  are  chosen  as  the  steady-state  Kalman  gains 
that  minimize  the  mean-square  error  in  the  position,  velocity,  and  acceleration  estimates. 
For  the  a,  0, 7  filter, 


rp 

Xk  =  [xk  Xf.  £*] 

,1  T  0.5T2 

Fk=  |  0  1  T 

.0  0  1 

1  T 


G*  = 


Tl 

2 

Hk  =  {  1  0 
Rk  =  <y\ 

Qk  =  <4 


T  1 

o] 


Kk  = 


oi  — 


P 

T 


T2 


ir 


(2.39) 

(2.40) 

(2.41) 

(2.42) 

(2.43) 

(2.44) 

(2.45) 


The  a,0, 7  gains  are  determined  as  in  [1,3,8]  by  solving  the  simultaneous  equations 

, r 2  v2 

r2  _  rpA  "w  7 

“  (1  -  a)  _ 

0  =  2(2  —  a)  —  4\/l  —  a 

02 


(2.46) 

(2.47) 

(2.48) 


The  steady-state  error  covariance  matrix  of  the  filtered  estimates  for  the  a,  0,~f  filter  is  given 
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in  [1,8] 


as 


^tit  = 


a 

0 

l_ 

T 

rp2 

0 

4  a0  +  l(0  —  2a  —  4) 

0(0-1 ) 

■T 

4(1  -  a)T2 

2(1  -a)T3 

2_ 

0(0  ~  7) 

l(0-lt) 

rp2 

2(1  -  a)T3 

(1  -  a)T4  J 

(2.49) 


The  steady-state  gains  and  error  covariance  in  Eqs.  (2.46)  through  (2.49)  are  derived  in 
Appendix  B. 

The  input-output  relationships  between  the  measurements  V*  and  filtered  state  A*)*  can 
be  expressed  as  a  linear  system  that  is  given  by 

T 2 


Xk\k  = 


l -a  (1  -a)T  (l-a)y 


-f  (1-/J) 


a  -  f  )t 


>-I 


a 

Xk-\\k~i  + 

0 

T 

7 

. T 2  . 

n 


(2.50) 


2 L  _1 

T2  T 

Using  Eq.  (2.50)  provides  the  steady-state  error  covariance  of  X ^  that  results  from  the 

measurement  errors.  That  error  covariance  Sa ^  as  given  in  [11]  is 

,2 


Sa^~  LdJ* 


d\d.2- 

2ad,2  —  ft2  (6  a  —  4)  +  a0j 
|(2o  -  0)(20  -  7) 
p(2<f2  +  D(l  -  2D)) 


£(2a-0)(20-7) 

^(l\2-a)+2D\D-l)) 


±(2ch  +  Dh  -  2D)) 


207 

T3 


201 

T3 


(2)8-7) 


(2/?  -  7) 
4/?72 


J>4 


(2.51) 


where  dj  =  4  —  2a  —  0  and  e?2  =  2a/?  -f  7(0  —  2).  The  measurement  error  variance  reduction 
ratios  of  the  filter  are  given  by  Eq.  (2.51)  when  cr2  =  1  with  the  (1,1),  (2,2),  and  (3,3) 
elements  of  Sap-y  denoting  the  position  variance  reduction  ratio,  velocity  variance  reduction 
ratio,  and  the  acceleration  variance  reduction  ratio,  respectively. 


Using  least-squares  estimation  in  conjunction  with  a  constant  acceleration  motion  model, 
a  simple  gain  scheduling  procedure  for  a,  0  and  7  during  initialization  is  given  by 

3(3)t2  +  3ifc  +  2) 


ak  =  max] 
0k  =  max] 


(*  +  l)(*  +  2)(*  +  3) 
18(2/:  +  1) 

(*+l)(fc  +  2)(fc  +  3) 
60 

(fc  +  l)(fc  +  2)(fc  +  3) 
2-7 


,0} 

1 7} 


(2.52) 

(2.53) 


7fc  =  max] 


(2.54) 
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with  A"o|_i  =  [0  0  0]^.  Note  that  if  Yo,i/T 2  can  be  extremely  large  relative  to  the 

dynamic  range  of  the  computer,  the  first  three  iterations  of  the  filter  should  be  computed 
analytically  to  use  a  three-point  initialization  procedure. 
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CHAPTER  3 

TWO-STAGE  KALMAN  ESTIMATOR 


Consider  the  problem  of  estimating  the  state  of  a  linear  system  in  the  presence  of  a 
random  bias  that  influences  the  system  dynamics  and/or  observations.  The  bias  vector  may 
represent  a  part  of  the  augmented  system  state  as  suggested  in  [12].  The  idea  of  using  a 
two-stage  filter  to  implement  an  augmented  state  filter  was  introduced  in  [12].  The  idea  is 
to  decouple  the  central  filter  into  two  parallel  filters.  The  first  filter,  the  “bias-free”  filter, 
is  based  on  the  assumption  that  the  bias  is  nonexistent.  The  second  filter,  the  bias  filter, 
produces  an  estimate  of  the  bias  vector.  The  output  of  the  first  filter  is  then  corrected 
with  the  output  of  the  second  filter.  It  was  shown  in  [12]  that  if  the  bias  is  deterministic 
and  constant,  but  unknown,  the  two-stage  filter  is  equivalent  to  the  augmented  state  filter. 
It  was  shown  in  [4,13]  that  under  an  algebraic  constraint  on  the  correlation  between  the 
state  process  noise  and  the  bias  process  noise,  the  proposed  two-stage  Kalman  estimator  is 
equivalent  to  the  augmented  state  Kalman  filter. 

Consider  a  linear  system  whose  dynamics  is  modeled  by 

Xk+i  =  FkXk  +  Gkbk  +  G%Wf  (3.1) 

h+i  =bk  +  GbkWbk  (3.2) 

where  Xk  is  an  n  dimensional  system  state  vector,  bk  is  a  p  dimensional  bias  vector.  This 
system  may  represent  the  dynamics  of  a  maneuvering  target,  where  the  position  and  velocity 
are  the  system  state  and  the  bias  represents  the  target  acceleration.  The  Wk  and  Wk  are 
white  Gaussian  sequences  with  zero  means  and  variances  given  by 


E\W?(W?)t]  =  Q?hl  (3.3) 

E[Wl(Wf)Tl  =  QUu  (3.4) 

e[w?(w<)t\  =  Qtkh,  (3.5) 

The  state  measurement  model  at  time  k  is  given  by 

Yk  =  HkXk  +  Ckbk  +  vk  (3.6) 
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where  V*  is  the  measurement  and  vk  ~  N(0,  Rk)  is  the  measurement  error.  The  standard 
approach  to  estimating  the  state  and  bias  is  to  form  an  augmented  state  model  which  includes 
both  the  bias  and  the  state.  With  this  augmented  state  model,  a  Kalman  filter  may  be  used 
to  produce  the  optimal  state  estimates. 

If  the  bias  term  is  ignored,  (6*  =  0),  the  bias-free  filter  is  the  Kalman  filter  based  on  the 

jy 

model  in  Eqs.  (3.1)  and  (3.6),  where  fictitious  statistics  are  used  for  Wk  (i.e.,  Qk  is  used 
instead  of  Gk  Qk  {Gk)T).  The  bias-free  filter  is  given  by 


fclifc-l  =  -Ffc-l^ib-llifc-l 

(3.7) 

xt[k  =  +  Kk[Yk  -  HkXk |,_,J 

(3.8) 

=  Fk-iPk^\k-iFti  +Qk- i 

(3.9) 

pk,k  =  U  -Kk 

(3.10) 

Xk  =  P^-iHjlH.P^Hl  + 

(3.11) 

where  Qk-\  is  defined  later  in  this  chapter.  The  represents  the  estimate  of  the  state 

_ x  _ 

process  when  the  bias  is  ignored,  and  Pk\k  is  the  error  covariance  of  Xk\k. 

As  in  [12],  a  separate  filter  may  be  used  to  estimate  the  bias  vector  from  the  residual 


sequence  of  the  bias-free  filter.  The  bias  filter  is  given  by 

h\k-i  =  (3.12) 

h\k  =  h\k-\  +  Kkir k  ~  Sifcfcjfci*-!]  (3.13) 

H\k- 1  =  4-llt-l  +  gL,qLi(gL1)t  (3.14) 

Ki  =  Pt^Sj  +  Hkpfik.t  Hi  +  Rk]-'  (3.15) 

=  (3-16) 

where 

Sk  =  HkUk  -f  Ck  (3.17) 

Uk  =  Fk-iVk-i  +  Gk-i  (3.18) 

Vt  =  (/  -  KkHk)Uk  -  ~KkCk  (3.19) 


and  rk  is  the  residual  of  the  bias-free  filter.  Initialization  of  this  bias  filter  is  discussed  later 
in  this  chapter. 

The  algorithm  for  compensating  the  output  of  the  bias-free  filter  with  the  output  of  the 
bias  filter  is  given  by 

%k\k  =  E[Xk]  =  ^*1*  +  Vkh\k  (3.20) 
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Pt[t  =  E[(Xt  -  Xtit)(Xt  -  Xiltf)  =  P*t\k  +  Vt%Vf  (3.21) 

Pill  =  -  Xtlt)(h  -  bklkf]  =  VtPty,  (3.22) 

%  =  E[(k  -  **|i)(*it  -  bt „)T]  =  Pt‘|t  (3-23) 

The  structure  of  the  two-stage  Kalman  estimator  is  shown  in  Figure  3-1. 

Theorem:  If 

G?Q?‘(Gt)T  =  t/HiGl<3t(Gt)T  (3.24) 

_ x 

and  the  error  covariance  Q k  of  the  process  noise  of  the  bias- free  filter  model  given  by 

Qk  =  GfQ}(G*f  -  Uk+lGbkQ\(Gbk)TU?+l  (3.25) 


is  positive  semidefinite,  then  the  filter  given  by  Eqs.  (3.20)-(3.23)  is  equivalent  to  the  aug¬ 
mented  state  filter. 

Proof:  See  [4,13]. 

While  the  algebraic  constraint  of  Eq.  (3.24)  will  not  be  ratisfied  by  almost  all  real 
systems,  the  theorem  provides  a  basis  for  assessing  the  degree  of  suboptimality  of  a  two- 
stage  estimator  when  applied  to  specific  problems.  For  example,  let  the  algebraic  constraint 
of  Eq.  (3.24)  be  expressed  as 

Gf«ifi(Gl)T<(l+f)U»+lC40l(Gi)7'  *>»  (3.26) 

Then  for  a  given  system,  the  smallest  positive  t  that  satisfies  Eq.  (3.26)  indicates  the  degree 
of  suboptimality  of  the  two-stage  estimate  with  respect  to  the  augmented  state  filter. 

For  initialization  of  the  two-stage  estimator,  P^  =  0  if  the  initial  estimate  of  the  bias 
is  uncorrelated  with  the  initial  estimate  of  the  bias-free  state.  If  P^fb  =  0,  Eqs.  (3.22)  and 
(3.19)  imply  that  Vo  =  0  and  Uo  =  0.  Note  that  as  shown  in  [4,13]  the  initialization  does 
not  require  =  0. 

For  the  system  and  bias  models  given  in  Eqs.  (3.1)  and  (3.2),  the  process  noise  covari¬ 
ances  are  given  in  Eqs.  (3.3)  through  (3.5).  However,  for  tracking  targets  with  the  two-stage 

estimator,  the  process  noise  covariances  in  Eqs.  (3.3)  through  (3.5)  are  design  parameters. 

_ x 

When  designing  the  two-stage  estimator  for  tracking  maneuvering  targets,  Qk  can  be  chosen 

to  obtain  the  desired  response  of  the  constant  velocity  filter  and  Qb>  can  be  chosen  for  m a- 

— X 

neuver  response.  In  this  type  of  design  procedure,  Qk  is  chosen  to  be  positive  semi-definite 
and  Q  is  allowed  be  a  free  parameter  that  depends  on  the  selection  of  Qk  and  Qbk. 
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Figure  3-1.  Structure  of  Two-Stage  Kalman  Estimator 
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CHAPTER  4 

TWO-STAGE  ALPHA-BETA-GAMMA  ESTIMATOR 

For  a  uniform  data  rate  and  stationary  noise  processes,  the  single  coordinate  version 
of  the  two-stage  Kalman  estimator  can  be  approximated  with  a  filter  including  three  fixed 
gains.  In  this  fixed-gain  filter,  the  two-stage  a,  /?,  7  estimator,  the  position  and  velocity  are 
tracked  in  the  first  stage,  which  is  a  standard  a,  0  filter,  and  the  acceleration  is  tracked 
separately  in  the  second  stage,  which  is  a  standard  single  gain  filter.  The  output  of  the 
acceleration  filter  can  be  used  to  adjust  the  estimates  of  the  a,  0  filter,  as  shown  in  Figure 
4-1,  when  a  maneuver  is  detected.  The  parameters  Ki,  K?,  and  Kz  will  be  defined  later. 

The  bias-free  filter  of  Eqs.  (3-7)— (3.1 1)  corresponds  to  the  standard  constant  velocity 
filter  in  a  single  coordinate  when 


F  [l  T 

Fk  ~  [0  1 

(4.1) 

Qk  =  ftGkGl 

(4.2) 

JT- 

II 

II 

(4.3) 

Hk  =  {\  0],  C*  =  0 

(4.4) 

KX -\  Pk]T 

Ai it  -  ak  y 

(4.5) 

for  scalar  acceleration  error  variance  q j£.  The  bias  filter  of  Eqs.  (3.12)-(3.16)  corresponds 
to  the  standard  constant  state  filter  with  residuals  of  the  bias-free  filter,  rk,  as  the  input 
and  Gbk  =  1.  Since  the  bias  corresponds  to  a  scalar  acceleration,  the  bias  filter  gain  will  be 
a  scalar.  Let  the  bias  filter  gain  be  denoted  as 

Kik=7kS;\  k>0  (4.6) 

where  7*  are  Sk  are  positive  scalars  obtained  from  Eqs.  (3.15)  and  (3.17).  Letting  bk  =  Ak 
and  inserting  Eq.  (4.6)  into  Eq.  (3.13)  gives 

Ak\k  =  +  JklS^r/c  -  A*|*_i]  (4.7) 
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Figure  4-1.  Two-Stage  a,  /?, 7  Estimator 
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Using  Eqs.  (4.1)  through  (4.7)  in  the  bias-free  and  bias  filters  gives  the  two-stage  a,/},  7 
estimator  as  summarized  by  the  following  equations: 


Prediction 


Xk\k-l  = 

Afc|*-l  =  ^fc-i|it-i 

1  -  a*:-i  ~  /?t- 1  T I 


Uk  = 


Pk-\ 

T 


Uk-i  +  Gk-i 


Measurement  Update 


T7X, 


Xk\k  =  Xk\k-i  +  Kk(Yk-HkXk\k_1) 
Ak\k  =  0  ~  7ifcMjt|fc-i  +  Tit  SkXrk 


Output  Correction 


(4.8) 

(4.9) 

(4.10) 


(4.11) 

(4.12) 


-^*|*  =  %k\k  + 

1  —  ak  0 

Vk  =  J± 

.  T 


1 


Uk 


(4.13) 

(4.14) 


where  the  gains  are  a  function  of  time.  In  steady-state  conditions,  ak  —  a,  0k  =  0,  and 

T* 

7it  =  7.  Also,  the  bias  filter  is  initialized  with  A0\_x  =  0  and  U- 1  =  [0  0]  . 


STEADY-STATE  RELATIONSHIPS 


If  the  measurement  rate  is  uniform  (i.e.,  T  is  constant),  the  measurement  error  statistics 
are  stationary  (i.e.,  Rk  =  cr^),  and  the  acceleration  error  statistics  are  stationary  (i.e.,  qk  = 
&1,),  then  the  bias-free  filter  in  steady-state  conditions  corresponds  to  an  a,  0  filter  with 
ak  =  a  and  0k  =  0.  Then  a  and  0  are  given  by  Eqs.  (2.29)  and  (2.30)  for  the  a,  0  as 


T  =  T 


p.7. 

4 


(i-«) 


0  =  2(2  —  a)  —  4%/l  —  a 


(4.15) 

(4.16) 


where  T  denotes  the  tracking  index  for  the  bias-free  filter. 


Since  the  statistics  of  the  residuals  rk  in  the  a,  0  filter  will  be  stationary  in  steady  state, 
the  bias  filter  will  achieve  steady-state  conditions  if  the  statistics  of  Wk  are  stationary  (i.e., 
Qk  =  cr^,).  The  bias  filter  in  steady-state  conditions  corresponds  to  a  single  gain  filter  with 

I<1  =  Kb  =  7  S'1  (4.17) 
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Q\  ~  aw)-  The  bias  filter  in  steady-state  conditions  corresponds  to  a  single  gain  filter  with 


r  o-l 


Ki  =  K  =  7  S' 


(4.17) 


where  S  is  a  constant  scalar  in  steady-state  conditions.  Using  Eqs.  (4.10)  and  (4.14),  the 
steady-state  parameters  are 


Uk+1  =uk  =  u  = 


v*+i  =  vk  =  V  = 


T 2  o 

j  T+0'5)r 


(j  -0.5 )T 


-,T 


The  steady  state  values  of  AT,  AT,  and  AT  in  Figure  4-1  are  given  by 

T2 

Ki" 


AT  =  5"1  =  (HkU)~'  = 


=  V 


(4.18) 

(4.19) 


(4.20) 

(4.21) 


Since  the  two-stage  a,  0,  7  estimator  is  an  a,  0  filter  when  the  switch  is  open,  the  steady- 

_ x  _ x 

state  error  covariance  Pk\k  =  P‘  is  provided  by  Eq.  (2.31)  as 


Pk\ jt  =  P 


a 


0_ 

T 


0  0(2a  -  0) 


(4.22) 


T  2(1  -  a)T 2 

The  variance  of  the  residuals  of  the  bias-free  or  a,  0  filter  is  given  by 

4es  =  HtFtPX  Fj  Hi  +  qlHtGtGlHl  + 

Inserting  Eqs.  (4.1)  through  (4.4)  and  (4.22)  into  Eq.  (4.23)  gives 

2  2m  r4  2 

°RES  -  “  +  2(~-^)l +  X17" 


(4.23) 


(4.24) 


Using  Eq.  (4.15)  to  eliminate  a \  from  Eq.  (4.24)  and  using  a2  —  20  +  a0  +  0.250  =  0  of  [8] 
gives 

"2RES  =  ^  (4.25) 

Thus,  using  Eq.  (4.25)  in  conjunction  with  Eq.  (4.20)  gives  the  variance  of  the  input  to  the 
second  stage  or  bias  filter  as  {K\)2cr2RES.  Using  Eqs.  (2.8)  and  (4.15)  gives  the  steady-state 
gain  for  the  second  stage  as 

7  =  Pk\k(fiaREs)  =  pb(f4°REs)  =  ^(^(f-o))  ^  = 
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Thus,  the  steady-state  error  covariance  of  the  second  stage  ;s  given  by 

?2  ,  /  fp 


Ph  =  y(fi^ss)  =  = 


,r4(l  -  a)- 

Inserting  Eq.  (2.4)  into  Eq.  (2.7)  for  the  bias  filter  gives 

P1  =  (1  -  ill l*  +  4! 

Thus 


7^1 


Pb  = 


(1  -7) 


Eqs.  (4.27)  and  (4.29)  give 


72 


*1 


(1  ~7) 

Using  Eqs.  (4.19)  and  (4.27)  in  Eq.  (3.22)  gives 


Pk\k  =  PXb  =  VPb  =  7<r2 


Using  Eqs.  (4.19)  and  (4.31)  gives 

VPhVT  = 


1  —  a 


£ 

T2 

(a  -  0.5 /3)P 
(1  -  a)T 3 

(a  -  0.5/?) 


(a  -  0.5/?)  (a  -  0.5/?)2 

T  (1  -a)T2 


(4.27) 

(4.28) 

(4.29) 

(4.30) 


(4.31) 


(4.32) 


Since  the  output  error  covariance  of  the  two-stage  estimator  is  dependent  on  the  maneuver 
switch,  let  the  switch  gain  be  denoted  by 

{0,  switch  open 

(4.33) 

7,  switch  closed 

Using  Eqs.  (4.22),  (4.27),  (4.31),  and  (4.32)  in  Eqs.  (3.21)  through  (3.23)  gives  the  output 
error  covariance  of  the  two-stage  estimator  as  a  function  of  the  maneuver  switch  to  be 


Pk\k(l,l) 

Pk\k(h2) 

Pk\k(  U3) 

^|*(2,2) 

^*(2,3) 

Pk\k(  3,3) 


^2(«  +  (1  -  a)Gs) 

a 


a 
T 
a2v0 


£[/?  +  (« -0.5 0)GS) 

Gs 

n  zr\ 

[/?  +  (a-0.5/?)Gs] 
GS 
Gs 


T 2 

2  (a-  0-5/g) 
CTt,(l  —  a)T2 
2  0(a  —  0.5/?) 
**  (1  -a)T3 

.2  02 


v{\  -  a)TA 


(4.34) 

(4.35) 

(4.36) 

(4.37) 

(4.38) 
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where  Pk\k(hj)  denotes  the  ( i,j )  element  of  the  error  covariance. 


SELECTING  GAMMA 


Since  the  algebraic  constraint  of  Eq.  (3.24)  will  not  be  satisfied,  the  two-stage  estimator 
with  the  maneuver  switch  closed  will  not  be  identical  to  the  augmented  state  niter  (i.e., 
the  a,j3, 7  filter).  However,  the  position,  velocity,  or  acceleration  variances  of  the  two-stage 
estimator  and  the  a,/3, 7  filter  can  be  matched  through  the  selection  of  7.  Using  the  (1,1) 
elements  of  Eqs  (2.49)  and  (4.34),  a  match  between  the  position  variances  is  given  by 


(1-a) 


(4.40) 


where  a  denotes  the  alpha  gain  of  a  standard  a,  0, 7  filter  that  is  desired  for  tracking  through 
maneuvers.  Using  the  ^2,2)  elements  of  Eqs.  (2.49)  and  (4.37),  a  match  between  the  velocity 
variances  is  given  by 


7  = 


/I  -a\  /d/?  +  0.257(/?-2o-4)\  0 

\  1  -  a  A  (a-  O.50)2  )  a-  0.50 


(4.41) 


where  a,  0, 7  denote  the  gains  of  a  standard  a,  0,~f  filter  that  is  desired  for  tracking  through 
maneuvers.  Using  the  (3,3)  element  of  Eq.  (2.49)  and  Eq.  (4.39)  gives  a  match  between  the 
acceleration  variances  as 


7  = 


708-7) 

02 


1  —  Q 


1-0 


(4.42) 


Fig.  4-2  gives  various  values  of  7  versus  Q  for  matching  the  position,  velocity,  or  accel¬ 
eration  variances  when  a  —  a  =  0.1.  Using  Fig.  4-2,  consider  the  two-stage  estimator  with 
a  =  0.3  and  7  =  0.2  and  the  a,0, 7  filter  with  d  =  0.4.  Then  the  position  and  velocity  esti¬ 
mates  of  the  two-stage  estimator  with  the  maneuver  switch  closed  will  closely  match  those 
estimates  of  the  corresponding  a,0, 7  filter,  while  the  acceleration  estimates  of  the  two-stage 
estimator  will  be  filtered  more  heavily  than  those  of  the  a,  /3, 7  filter.  Fig.  4-2  also  shows 
that  the  estimates  of  two-stage  estimator  for  a  —  a  =  0.1  will  more  closely  match  those  of 
the  augmented  state  filter,  the  a,  0 , 7  filter,  when  0.4  <  a  <  0.5  and  0.25  <  7  <  0.5. 


MEASUREMENT  VARIANCE  REDUCTION  MATRIX 

The  variance  reductions  between  position,  velocity,  and  acceleration  estimates  and  the 
measurements  are  often  considered  in  the  design  and  analysis  of  fixed-gain  filters.  The 
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0.2  0.3  0.4  0.5  0.6  0.7  0.8 

ALPHA 

Figure  4-2.  7  Versus  a  for  a  —  a  =  0.1 


variance  reduction  matrix  for  the  two-stage  ot,(3, 7  estimator  is  derived  by  considering  the 
estimator  with  the  maneuver  switch  open  as  a  linear  system  with  a  white  noise  input.  The 
variance  reduction  matrix  for  the  estimator  with  the  maneuver  switch  open  is  used  to  com¬ 
pute  the  variance  reduction  matrix  for  the  estimator  with  the  maneuver  switch  is  closed. 

_ T  T 

The  input-output  relationships  between  the  measurements  and  when  the 

maneuver  switch  is  open  can  be  expressed  as  a  linear  system  that  is  given  by 

Xk\k  _  ~p  +  QYk  (4.43) 

4*1*  Ak-i\k-i  J  *  ' 

where 

"  1  —  ot  (1  -a)T  0 

F  =  ~f  (1-0)  0  (4.44) 

70  70  1  - 

t  ^  ^ 

a 

0  ,  x 

f  (4-45) 

70 

T2 . 

r _ *  T 

Using  Eq.  (4.43)  provides  the  error  covariance  of  that  results  from  white 
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noise  measurement  errors.  That  error  covariance  is  denoted  as 


E 


Xk 

Ak 


-T 

'I  ^ 


- 

- 


and  given  by 


—  -7fO  —T  .  —  —T  2 


=  F  s:t,F-  +GG-  cl 


(4.46) 

(4.47) 


where  <r2  is  the  variance  of  the  input  V*.  Eq.  (4.47)  can  be  used  to  solve  for  elements  of 
in  terms  of  the  filter  gains,  measurement  period,  and  the  measurement  error  variance.  Let 


?0 


511 

512 

513 

= 

512 

522 

523 

.513 

523 

533  . 

(4.48) 


Since  the  position  and  velocity  estimates  of  the  two-stage  estimator  with  the  maneuver 
switch  open  are  equivalent  to  those  of  the  a,  ft  filter,  sn,  S12,  S22  are  given  by  those  of  the 
a,  fi  filter  in  Eqs.  (A.43)  through  (A. 45)  as 


2a2  +  /?(2  —  3a  2 
511  =  a[4  -  2a  -  ft  ^ 
(2a  -  0)0  _2 


512  = 


522  = 


a[i-2a-(3]T  v 
2/32 


a[4  —  2a  —  P]!*2  v 
Equating  the  (1,3),  (2,3),  and  (3,3)  elements  of  Eq.  (4.47)  gives 

(a~  1)5/?  r«  __  1  ot„  1  t2 


513  = 


T2 


[511  +  2Ts\2  +  T'£S2i]  +  (7  ~  !)(<*  —  1)[-*13  +  Ts23\  + 


(4.49) 

(4.50) 

(4.51) 

(4.52) 


s23  =  7^3  [511  +  2 Tsi2  +  T2s22]  -  ^(512  +  ^22]  +  ^  [si3  +  T S23]  +  ^~<T2  (4.53) 


533(2  -  7)7  =  ~^[5ll  +  2Tsn  +  T2s22 3  +  ^%~%13  +  TS2Z\  +  ~^~2 


J ’4 

Using  Eqs.  (4.49)  through  (4.51)  gives 


j-4  av 


nrri  _2  ,  r2a2  +  af3  +  2/?i 

+  2rS12  +  T  322  =  L[4-2a-)Jj] 


_2  f  ilia  +  2a)  1 

S,2  +  T322  =  «t„[-[4—  -)rJ 

Adding  Eqs.  (4.51)  and  (4.52)  and  using  Eqs.  (4.55)  and  (4.56)  gives 
513  +  Ti S23  = 


(4.54) 

(4.55) 

(4.56) 


D[0  +  7(0  lJ[Jn  +  2rsi,  +  r2J22]  _  £[s,2  +  Ja22] 


+  + tS23]  +  **+22Lj 


y2 


=  a 


<fh 

v  J>2 


2-ya2  +  (7  +  2 )aj3  +  /?(/?  -  27) 

[a[(l  -  a)72  +  (a  -  /?)7  +  /?J[4  -  2a  -  0}\ 


(4.57) 

(4.58) 
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Inserting  Eqs.  (4.55)  and  (4.58)  into  Eq.  (4.52)  gives 


2  07 

3 13  rri') 


27a2  +  (2  -  7 )a/3  +  [(/?  -  2)7  -  0]0 


’T2  [a[(l  -  a)72  +  (a  -  0)i  +  0}[\  -  2a  -  /3]J 
Inserting  Eqs.  (4.55)  and  (4.58)  into  Eq.  (4.53)  gives 


(4.59) 


^23  =  &v 


027 


7(2a-/3)  +  2/3 


T3  [a[(l  -  a)72  +  (a  -  0)*f  +  0][4  -  2a  -  0]\ 
Inserting  Eqs.  (4.55)  and  (4.58)  into  Eq.  (4.54)  gives 

2/?272  f  7(2a-/3)  +  2  0 


s  33  = 


(2  -  7)^ 


a((l  -  a)72  +  (a  -  0)^  +  0][4  -  2a  -  /?]J 


(4.60) 


(4.61) 


Inserting  Eqs.  (4.49)  through  (4.51)  and  Eqs.  (4.59)  through  (4.61)  into  Eq.  (4.48)  provides 
the  steady-state  error  covariance  of  the  filter  output  that  results  from  the  measurement  errors 
when  the  maneuver  switch  is  open.  The  elements  of  are  summarized  as 


M)  = 

s2"(  1.2)  = 

SS),(1,3)  = 
S2n(  2,2)  = 
S?«(2.3)  = 

^a44(3> 3)  — 


4-[2a2  + 13(2  ■ 

aai 

-3a)] 

(4.62) 

°lP  ,9/v 

arf,r(2a 

-0) 

(4.63) 

ad\dzT 2 

27  a2 
0 

+  a(2 

-7) 

+  7(0-2  )-/*] 

(4.64) 

<r2  202 
adjT2 

(4.65) 

avl02  ,-(n 

adidsT*^  a 

-0)  + 

20) 

(4.66) 

a2Xl202 

a(2  -  7)did3r4 

(7(2a 

-0) 

+  20) 

(4.67) 

where  d\  —  4  —  2a  —  (3  and  d3  =  (1  —  a)72  -J-  (a  —  0)~i  +  (3. 


The  input-output  relationships  between  the  measurements  V*  and  filter  output  when 
the  maneuver  switch  is  closed  can  be  expressed  as 


'**1* 


=  D 


X 


*|* 

1*|* 


(4.68) 


where 


T2  I 

1  0  (1-a)- 

0  1  —  0.5)T 

0  0  1 


(4.69) 
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Using  Eq.  (4.68)  provides  the  steady-state  error  covariance  that  results  from  the  measure¬ 
ment  errors  when  the  maneuver  switch  is  closed.  That  error  covariance  is  given  by 

s£n  =  DS°fi,DT  (4.70) 


INITIALIZATION 

A  gain  scheduling  procedure  is  needed  to  facilitate  the  settling  of  the  estimates  of  the 
two-stage  a,  /?,  7  estimator.  A  gain  scheduling  procedure  that  produces  estimates  that  are 
equivalent  to  those  of  a  quadratic  least-squares  fit  is  developed.  The  estimates  will  be  shown 
to  be  equivalent  to  those  produced  by  an  a,  {3, 7  filter  that  is  initialized  with  gains  scheduled 
with  Eqs.  (2.52)  through  (2.54). 


Since  the  bias-free  filter  is  an  a,  /3  filter,  Eqs.  (2.37)  and  (2.38)  will  be  used  to  schedule 
a*  and  /4  as 


,  2(2* +1) 

“*  =  max((*TT)(*T2)’“) 

ft"max((*  +  1)6(*  +  2)'^ 

with  X0|_i  =  [0  0]T.  Inserting  Eqs.  (4.71)  and  (4.72)  into  Eq.  (4.10)  gives 


(4.71) 

(4.72) 


r  fc-4 

1 

T 

-T2- 

£4  = 

k 

6 

1 

£4-i  + 

2 

L  k{k  +  1)T 

.  T  . 

A  solution  to  the  time- varying  difference  equation  in  Eq.  (4.73)  is 


14  = 


(k  +  l)(fc  +  2)T2 
12 

(k  +  l)T 

2 


(4.73) 


(4.74) 


Inserting  Uk-i  from  Eq.  (4.74)  into  Eq.  (4.73)  confirms  that  Eq.  (4.74)  is  a  valid  solution 
of  the  difference  equation.  Using  Eqs.  (4.71),  (4.72),  and  (4.74)  in  Eq.  (4.14)  gives 


Vk  = 


k{k-l) 

6 


(jfc  +  i)(jfc  +  2)r 


O' 

'  k{k-l)T2' 

£4  = 

12 

1 

kT 

- 

2 

(4.75) 
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Using  Eq.  (4.75)  with  Eq.  (4.13)  gives 


^*|*  = 


x*|*  I  _ 

=  **|*  +  ^4^*1*  = 


k(k  -  1  )T2  . 

x*|*  + - ^2 - Ajfcl* 

kT 

*>*|fc  +  ~2~Ak\k 


(4.76) 


Delaying  the  time  index  in  Eq.  (4.76)  gives 


^*-il*-i  = 


.  (k-l)(k-2)T2  A 

x*-l|*-l  +  - 77 - "A— l|t— 1 

(*  —1)7’ 

u*-l|fc-l  + - 2 - ^*—11*—! 


(4.77) 


Also, 


T2  .  _  (k  +  2 ){k  +  3)T2  . 

xk-i|*-i  +  Txk_x  !*_!  +  -^Ak_x\k_y  =  +  T  V/t_i|fc_i  + - — - 

(4.78) 

Using  Eqs.  (4.74)  and  (4.78)  in  Eq.  (4.12)  gives 

=  (1  -  7*)4*_i|jt_i  +  lk{HkUk)~X\Yk  ~  x*-i|*-i  “  7v*—i|*— l] 

=  'U-m-i  +  ^(jkT 7~7zyn) x 

\v  -  Til  (t+l)((r  +  2)T2  1 

**  -  xfc-l|*-i  -  J  Vfc-ilfc-i - jr; - Ajt_i|*_iJ 


=  4jt_i|*_i  +  ( 


127* 

(k  +  \)(k  +  2)T2 


)  H  -  x*-l|*-l  ~  rx*-l|fc-l  ~  -7jTAk-\\k-l]  (4-79) 


which  is  in  the  form  of  the  acceleration  update  of  the  a,  /?,  7  filter.  Setting  the  gain  equal  to 
the  acceleration  gain  of  the  a,/3, 7  filter  in  Eq.  (2.54)  gives 

127*  60  iA  an\ 


Thus, 


Inserting  Eq.  (4.81)  into  Eq.  (4.79)  gives 


4jfc|*  =  i|fc— i  +  ( 


(lfc  +  l)(/fc  +  2)(ifc  +  3)r2 


60 

(*+l)(fc  +  2)(*  +  3) 

(4.80) 

5 

'  (it +  3) 

(4.81) 

[**  ~~  Xjt~1lfe-1  ~  ^*-11*-!  “ 

HAl  ,1 

2  ^*— 1|*— lj 

(4.82) 

which  is  the  acceleration  update  equation  for  the  a,/?, 7  filter  when  using  Eq.  (2.54)  for 
initialization. 
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Using  Eqs.  (4.11),  (4.12),  and  (4.74)  in  Eq.  (4.77)  gives 

kT 

*Jfc|Jb  =  ^fc|fc  +  ~Y^k\k 

=  +  Y^k  ~  ^k-ilk-l  ~  Tvk-ilk-i] 

kT  kT 

+  — (i  -  Tk)Ak-i\k-i  +  YikiHkUk)-1^  -  **_,,*_!  -  rtifc-u*-,] 
(*  —  1)T  rrA  3(2k  +  l)T  A 

=  vk-i\k-i  + - ^ - Ak-l\k-l  +  1  -Afc-ilfc-i - 2(fcT'3) . Ak~1\k~1 


+  ((Jfc  +  l)(k  +  2)T  +  (*  +  l)(*  +  2)(*  +  3)r)[F*  X*_1|*_1 

{  18(2*  +  1)  \ 

-  **-i|*-i  +  TAk~i\k~i  +  +  +  2)(jfc  +  3 )t)  X 

\Y  -  TT]  (k  4- 1)(*  +  2)T2  i 

Yk  ~  *jfc-l|ifc-l  -  1  vk-l\k-l - ^2 - A*-l|Jfc-l] 

,  (  18(2* +  1)  \ 

-  **-i|*-i  +  TAk_ ,|*_1  +  [(k  l)(k  2wfc  3]T)  x 


,  (  18(2* +  1)  \ 

-  **-i|*-i  +  TAk_ ,|*_i  +  {(k  +  +  2)(jb  +  3 )t)  X 

r  T2 

[Yk  ~  l  ~  Txk-i\k-i  -  Y^jfc-ilt-iJ  (4-83) 

which  is  in  the  form  of  the  velocity  update  of  the  a,  /?,7  filter  when  using  Eq.  (2.53)  for 
initialization. 

Using  Eqs.  (4.11),  (4.12)  and  (4.74)  in  Eq.  (4.77)  gives 
*(*-  1)T2  . 

xJt|Jfc  =  xk\k  + - — - Ak\k 

*(*-l)T2 

=  Zk-ilk-i  +  Tvk-ilJk-i  + - - (1  ~  7ifcM*-i|*-i 

+  («t  + [n  -  -  7Vim’ 


=  +  Tvk_1ik_1  + 


(*  +  l)(*  +  2)r2 


^  .  12  'lfc-l|*-l  12(fc  +  3) 

,  t  3(3*2  +  3*  4-  2)  \r„  _  ^  1 

+  l(jb  +  i)(fc  +  2 )(*  +  3 )t)  lYk  Xk-'\k-'  Tvk-nk-i\ 

,  T2  A  (  3(3*2  +  3*  +  2) 

-  *k\k  +  **-H*-i  +  2  Ak~l]k -1  +  V(*  +  1)(*  +  2)(*  4-  3 )TJ  X 

Ur  -  rr-  (*+l)(*  +  2)r2  „  ! 

-  z*-lj/fc-l  -  1  - J2 - At— 

.  T2  A  (  3(3*2  4-  3*  4-  2)  \ 

-  xk\k  +  **-i|*-i  +  2  Ak-'\k~l  +  \{k  +  l)(fc  +  2 )(*  +  3)77  x 

r  T2 

[H  _  -  Txk-\\k-i  ~ 


3(3*2  +  3*  4-  2  )T2 


(4.84) 
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which  is  in  the  form  of  the  position  update  of  the  a,  /?,  7  filter  when  using  Eq.  (2.52)  for 
initialization. 

Using  the  definitions  of  the  parameters  Ai,  A2,  and  A3  in  Eqs.  (4.20)  and  (4.21)  and 
Eqs.  (4.71),  (4.72),  (4.74),  (4.75),  and  (4.81),  the  gain  scheduling  is  summarized  as 


t  2(2  k  +  1) 

“‘  =  max((<t+l)(*  +  2)’a} 

(4.85) 

A-max!(*+l)(*  +  2)’/S} 

(4.86) 

1'*  =  maX<  (1  +  3)’ 11 

(4.87) 

A'1  =  ^max{(i  +  l)(ib  +  2)’« 

(4.88) 

K2  =  rWV\(1-“>} 

(4.89) 

K,  =  T  min{ 2  -  0.5} 

(4.90) 

with  A0|_!  =  [0  0  0]T. 

Figure  4-3  gives  a  comparison  of  the  Root-Mean  Square  Errors  (RMSE)  during  intial- 
ization  of  the  two-stage  a,  (3, 7  estimator  with  Eqs.  (4.85)  through  (4.90),  and  the  a,  ,#,7 
filter  with  Eqs.  (2.52)  through  (2.54).  For  the  a,  (3, 7  filter,  the  steady-state  gains  were 
chosen  with  a  =  0.45.  For  the  two-stage  estimator,  the  steady-state  gains  were  chosen  with 
a  =  0.35  and  7  =  0.15  to  match  the  position  variances.  The  errors  are  the  average  over  50 
experiments.  The  target  trajectory  began  at  12  km  with  an  initial  velocity  of  —100  m/s  and 
the  acceleration  remained  constant  at  10  m/s/s.  The  measurement  period  was  0.25  sec  and 
<rv  =  8  m.  The  RMSE  of  the  two  filters  losely  matched  in  the  transient  region  from  0 
to  4  sec,  while  the  RMSE  in  position  are  closciy  matched  in  steady-state. 

When  Eqs.  (4.85)  through  (4.90)  are  used  to  initialize  the  two-stage  estimator,  the 
maneuver  switch  can  be  left  open  until  enough  measurements  are  received  so  that  the  accel¬ 
eration  estimate  is  settled  before  it  is  used. 

EXAMPLE 

A  simple  tracking  system  is  considered  to  illustrate  the  operation  of  the  two-stage  es¬ 
timator.  The  tracking  system  measures  a  target’s  position  at  4  Hz  with  errors  that  have  a 
standard  deviation  of  8  m.  The  target  is  expected  to  perform  maneuvers  with  up  to  2  g  of 
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Figure  4-3.  RMSE  During  Initialization 
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acceleration.  For  an  or,/?, 7  filter,  a  =  0.45,  0  =  0.13  and  7  =  0.02.  For  the  two-stage  esti¬ 
mator,  a  =  0.35,  0  =  0.075  and  7  =  0.15,  where  Eq.  (4.40)  was  used  to  compute  7.  While 

various  techniques  can  be  used  for  maneuver  detection,  a  soft  switch  approach  was  chosen. 
For  the  soft  switch  approach,  the  bias  filter  is  duplicated  in  the  maneuver  detector  with  bk 
as  the  filter  output.  Then  the  gain  of  the  maneuver  switch  in  Eq.  (4.33)  is  augmented  by 

{0,  \bk\  <  4.0 

1.0,  IM>6.0  (4.91) 

0.5(|6jt|  —  4.0),  otherwise 

Also,  to  improve  maneuver  response,  the  magnitude  of  bk  is  restricted  so  that  \bk\  <  6.2. 
Using  the  soft  switch  approach,  the  correction  of  the  output  of  the  bias-free  filter  with  the 
acceleration  estimates  is  multiplied  by  Gss •  Simulated  tracking  results  from  a  Monte  Carlo 
simulation  with  200  experiments  are  given  in  Figure  4-4  for  the  two  filters,  where  the  target 
moved  with  constant  velocity  except  for  a  2  g  maneuver  from  15  to  30  sec.  A  critically 
damped,  second  order  system  with  an  approximate  time  constant  of  1  sec  was  used  to  model 
the  dynamics  of  the  target.  As  indicated  in  Figure  4-4,  the  two-stage  estimator  provides 
significantly  better  tracking  when  the  target  moves  with  constant  velocity,  while  matching 
the  tracking  performance  of  the  a,  0, 7  filter  during  the  maneuver. 
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Figure  4-4.  RMSE  in  the  Estimates 
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CHAPTER  5 

TWO-STAGE  ALPHA-BETA-GAMMA-LAMBDA  ESTIMATOR 


In  the  two-stage  a,  0,  7  estimator,  the  position  and  velocity  are  tracked  in  the  first  stage, 
which  is  a  standard  a,  0  filter,  and  the  acceleration  is  tracked  separately  in  the  second  stage, 
which  is  a  standard  single  gain  filter.  However,  when  tracking  two  or  three  dimensional 
target  motion  with  two-stage  a ,  0, 7  estimators  operating  separately  in  each  coordinate, 
the  state  estimates  will  be  biased  for  targets  maneuvering  with  constant  speed  because  the 
accelerations  are  time-varying.  This  bias  in  the  state  estimates  can  be  reduced  through  the 
use  of  the  kinematic  constraint  for  constant  speed  targets  as  in  [9].  The  speed  of  a  target  is 
given  by 


S  =  (i2  +  y2  +  i2)5 


(5.1) 


For  a  target  moving  at  constant  speed, 


(5.2) 


or 

(xi  +  yy  +  zz)  =  0 


(5.3) 


This  kinematic  constraint  can  be  incorporated  in  the  system  state  of  Eq.  (2.1)  or  used  as  a 
pseudomeasurement  in  conjunction  with  the  measurement  equation.  While  both  approaches 
are  feasible,  the  second  approach  is  more  attractive  because  the  first  changes  the  state 
equation  from  linear  to  nonlinear.  In  the  implementation  of  the  extended  Kalman  filter, 
including  nonlinearities  in  the  measurement  equation  is  computationally  less  expensive  than 
in  the  state  equation  [9].  Thus,  the  kinematic  constraint  of  Eq.  (5.3)  is  incorporated  into  the 
filter  through  a  pseudomeasurement  as  discussed  in  [9].  The  pseudomeasurement  equation 
is  given  by 

-^■Ak  +  fik  =  0  (5.4) 

where 


%  =  [**!*  ^*1*  **1*1 


(5.5) 
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$k\k  -  {i\\k  +  Vk\k  +  (5-^) 

and  /z*  ~  7V(0,  /?£).  The  V*|*  and  S*|*  are  the  filtered  velocity  and  speed,  respectively,  for 
time  k  given  measurements  through  time  k.  The  /z*  is  a  white  Gaussian  error  process  that 
accounts  for  the  uncertainty  in  both  Vk\k  and  the  constraint.  Since  the  initial  estimate  of  V*|* 
may  include  a  significant  error,  R jj!  is  initialized  with  a  large  value  and  allowed  to  decrease 
as 

fl£  =  n(£)*  +  r0  (5.7) 

where  0  <  S  <  1,  rj  is  a  constant  chosen  large  for  initialization,  and  r o  is  a  constant  chosen 
for  steady-state  conditions. 

The  two-stage  a,  ft,  7  estimator  utilizing  this  constraint  in  the  second  stage  will  be  called 
the  two-stage  a,  ft,  7,  A  estimator.  Eq.  (5.4)  can  be  written  as 

—  1  X*|* 

■c- (yk\kVk  +  h\k~zt)  =  ■=— ik  +  n  (5.8) 

Thus,  the  filtered  acceleration  estimates  can  be  inserted  into  the  left  side  of  Eq.  (5.8) 
to  compute  a  measurement  for  updating  the  x-coordinate  acceleration.  For  the  two-stage 
approach,  the  measurement  equation  for  the  x-coordinate  is  given  by  Eq.  (3.6)  with  6*  =  A* 
as 

Yk  I  r  , 

-g^(yk\kiik\k  +  zk\kzk\k)  [0  oj  Xk  +  [c%  Ak  +  [^*J  (5-9) 

Cl  =  (5.10) 

where  V*  is  the  x-coordinate  position  measurement,  v*  ~  N(0,  f?*)  is  the  measurement  error, 
/z*  ~  N( 0,  R^)  is  the  constraint  error,  and  A*  is  the  x-coordinate  acceleration. 

Since  the  bias-free  state  is  not  observed  through  the  pseudomeasurement,  its  gain  in  the 
bias-free  filter  can  be  shown  to  be  zero.  Thus,  the  bias-free  filter  can  be  defined  as  if  the 
kinematic  constraint  is  not  being  used.  Thus,  as  in  Chapter  4,  the  bias-free  filter  of  Eqs. 
(3.7)— (3. 11)  corresponds  to  the  standard  constant  velocity  filter  when  defined  as  in  Eqs. 
(4.1 )— (4.5).  Using  Eqs.  (3.1 7)— (3. 19)  in  conjunction  with  Eq.  (5.2),  f/*,  V*,  and  the  first 
element  of  5*  can  be  shown  to  be  independent  of  the  constraint  and  equivalent  for  both  the 
two-stage  q,  ft,  7  estimator  and  the  two-stage  a,  ft,  7,  A  estimator.  The  second  element  of  5*  is 
given  by  5*  =  C*.  Also,  since  the  errors  on  the  measurement  and  the  constraint  are  assumed 
to  be  independent,  the  measurement  update  c.nd  the  constraint  update  of  the  bias  filter  will 
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be  performed  separately  with  the  measurement-updated  acceleration  estimates  being  used 
to  compensate  the  velocity  estimates  in  the  pseudomeasurement.  Thus,  the  bias  filter  of 
Eqs.  (3.12)— (3.16)  corresponds  to  the  standard  constant  state  filter  with  the  residuals  of  the 
bias-free  filter  rk  and  the  pseudomeasurement  as  the  inputs.  Since  the  bias  corresponds  to  a 
scalar  acceleration,  the  bias  filter  gain  includes  a  scalar  gain  for  the  bias-free  filter  residual 
and  a  scalar  gain  for  the  constraint.  Let  the  bias  filter  gain  for  the  bias-free  filter  residuals 
be  represented  by 

Kbk=ik(Slr\  k>  0  (5.11) 

where  7*  and  Sk  are  positive  scalars  obtained  from  Eqs.  (3.15)  and  (3.17)  with  Sk  being  the 
first  element  of  Sk.  Let  the  gain  for  the  constraint  be  represented  by 

I<ck  =  AjfcCf ,  k  >  0  (5.12) 

where  Xk  is  a  positive  scalar.  Letting  Ak  =  bk,  the  two-stage  a,  /?,  7,  A  estimator  is  giv«*n  in 
the  following  equations: 

Prediction 


Uk 


Fk-iX  k-i\k-l 
'1  -  “  Pk-l 

0k- 1 

T 


Tl 


1 


14-1  +  <4-1 


Measurement  Update 


Xk\k  =  Xk\k-i  +  Kk(Yk-HkXk]k_l) 
^*iJfc  =  0  -  7*M*|*-i  +  lk(Sl)~\rk) 


Constraint  Update 

Cz 

Ak\k  =  ^*1*  -  ^k-^-{ik\k'ik\k  +  y*|ityjtn  +  zk\k'ik\k) 

Output  Correction 


^*1*  =  -*4|*  + 


Vk 


1  -  ak 

T 


0 

1 


Uk 


(5.13) 

(5.14) 

(5.15) 


(5.16) 

(5.17) 

(5.18) 

(5.19) 

(5.20) 
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The  velocities  of  Eq.  (5.18)  are  temporarily  compensated  with  the  acceleration  estimates  as 

in  Eq.  (5.11)  before  updating  the  acceleration  with  the  kinematic  constraint.  The  bias  filter 

T 

is  initialized  with  yl0|— l  —  0  an<^  =  [0  0]  . 

If  the  measurement  rate  is  uniform  (i.e.,  T  is  constant)  and  the  statistics  of  the  measure¬ 
ment  and  acceleration  errors  are  stationary  (i.e.,  Rk  =  a\  and  qk  —  d^,),  then  the  bias-free 
filter  in  steady-state  conditions  corresponds  to  an  a,  A  filter  with  a*  =  a  and  Ak  =  A-  The 
steady-stale  values  of  Uk  and  14  are  given  by  Eqs.  (4.18)  and  (4.19),  respectively.  Using 
Eqs.  (3.17)  and  (5.9)  gives 


<y  -  f  T'2 
*"  A 


(5.21) 


where  the  first  element  of  Sk  reaches  a  steady-state  value.  The  bias  filter  is  a  two-gain  filter 
with  the  residual  of  the  bias-free  filter  multiplied  by  (■5jJ)_1  and  the  pseudomeasurement  as 
inputs.  Using  Eq.  (2.8)  for  the  Kalman  gain  gives 


pb  d-2 
rk\kaw 

pb  r>xn- 2 
rk\k^kan 


(5.22) 


(5.24) 


where  cr^  is  the  variance  of  the  residual  errors  entering  into  the  bias  filter  as  shown  n  Eq. 
(4.27),  and  0^  is  the  variance  of  the  constraint  errors.  Thus,  using  Eq.  (5.22)  gives 

A*  =  4™  (5.23) 

and  the  output  variance  of  the  second  stage  is  given  by 

pb  —  -v  / c  04 'k 

k\k  7k  w  (  •  ) 

If  the  statistics  of  r*  are  stationary,  the  bias  filter  will  achieve  steady-state  conditions  when 
the  statistics  of  Wk  are  stationary  (i.e.,  Qbk  =  <7^)  an(j  the  kinematic  constraint  is  not 
included.  Thus,  for  simplification,  the  gain  7 k  will  be  assumed  to  reach  a  steady-state  value 
7.  Thus,  the  covariance  matrix  of  the  output  of  the  two-stage  a,  A,  7,  A  estimator  will  the 
equivalent  to  that  of  the  two-stage  a,  Ail  estimator  given  in  Chapter  4. 

When  selecting  A,  the  scalar  relationship  between  A  and  7  of  Eq.  (5.23)  can  be  adjusted 
through  a ^  to  tune  the  filter.  The  gain  scheduling  technique  for  initializing  the  twe-stage 
a.  Ail  estimator  given  in  Chapter  4  can  be  used  for  the  two-stage  a,  A,  7,  A  estimator  with 


6  <  1 


(5.25) 


>  —  ck  ,  1  ’  "  ^  ‘ 

cr-  "xb*  +  1 

where  ro  is  chosen  for  steady-state  conditions,  rj  is  chosen  large  for  initialization,  and  8  is 
chosen  for  the  desired  settling  time. 
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CHAPTER  6 

SIMULATION  RESULTS 


As  an  example,  a  tracking  system  that  measures  a  target’s  position  at  2  Hz  with  errors 
that  have  a  standard  deviation  in  range  of  6  m  and  standard  deviation  in  bearing  and 
elevation  of  2  mrad.  For  an  a,/?, 7  filter,  a  =  0.40,  (3  =  0.1  and  7  =  0.013.  For  the  two-stage 
estimator,  a  =  0.3,  (3  =  0.053,  and  Eq.  (4.41)  was  used  to  compute  7  =  0.19  to  match  the 
velocity  variances  of  the  filters.  For  Eq.  (5.25),  ro  =  0.75,  rj  =  400,  and  6  =  0.75.  Thus, 
in  steady-state  conditions  A  =  0.14.  While  various  techniques  can  be  used  for  maneuver 
detection,  a  soft  switch  approach  was  chosen.  For  the  soft  switch  approach,  the  acceleration 
filter  in  each  coordinate  is  duplicated  in  the  maneuver  detector  with  6*.  as  the  filter  output. 
As  a  result,  the  gain  for  compensating  the  a,f3  filter  in  each  coordinate  is  given  by 

I  0,  \bk\  <  4.0 

GSs  =  |  1.0,  IM  >  6.0  (6.1) 

(  0.5(|&*|  —  4.0),  otherwise 

Also,  to  improve  maneuver  response,  the  magnitude  of  6*  is  restricted  so  that  |6*|  <  6.2. 
Using  the  soft  switch  approach,  the  correction  to  the  output  of  the  bias-free  filter  with  the 
acceleration  estimates  is  multiplied  by  Gss ■  Simulated  tracking  results  from  a  Monte  Carlo 
simulation  with  200  experiments  are  given  in  Figure  6-1  for  the  a,f3, 7  filter  and  the  two- 
stage  estimator  with  and  without  the  kinematic  constraint.  For  this  example,  the  target 
moved  with  constant  velocity  except  for  a  4  g,  constant  speed  turn  from  12  to  24  s.  The 
target  began  at  a  range  of  17.5  km  and  moved  at  a  constant  speed  of  300  m/s  to  a  final  range 
of  9.3  km.  A  cr  ically  damped,  second  order  system  with  an  approximate  time  constant 
of  1  s  was  used  to  model  the  dynamics  of  the  target.  Both  two-stage  estimators  provide 
better  tracking  than  the  a,/?, 7  filter  when  the  target  moves  with  constant  velocity,  while 
the  two-stage  estimator  with  the  kinematic  constraint  provides  the  best  tracking  through 
the  maneuver. 
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Figure  6-1.  RMSE  for  Radar  Tracking  Example 


6-2 


NSWCDD/TR-92/297 


CHAPTER  7 

CONCLUSIONS  AND  FUTURE  RESEARCH 


The  tracking  performance  of  the  two-stage  estimator  can  be  achieved  by  operating  an 
a,  (3  filter  in  parallel  with  a  a,  /?,  7  filter  and  selecting  the  output  of  one  of  the  filters.  However, 
when  using  the  soft  switch  approach  to  maneuver  detection  and  response  as  presented  in 
Chapters  3  and  5,  a  second  ar,/3, 7  filter  is  required  so  that  its  acceleration  estimate  can 
be  restricted  during  a  maneuver  to  improve  detecting  the  end  of  a  maneuver.  While  this 
multiple  filter  approach  is  feasible,  it  requires  approximately  three  times  the  number  of 
computations  for  essentially  the  same  tracking  performance. 

Since  the  two-stage  a,  /3, 7  estimator  is  simple,  it  can  be  implemented  in  current  sys¬ 
tems  with  a  modest  increase  in  the  computational  burden  and  memory.  The  two-stage 
a,  (3, 7  estimator  has  the  potential  to  provide  significant  performance  gains  in  the  tracking  of 
maneuvering  targets  within  systems  that  are  responsible  for  tracking  and  engaging  a  large 
number  of  targets.  Also,  since  the  two-stage  a,/?, 7  estimator  maintains  an  a,/3  filter  track 
at  all  times,  the  command  and  control  decisions,  as  well  as  maneuver  detection,  can  be 
made  with  greater  consistency  with  the  two-stage  estimator  than  other  adaptive  filtering 
techniques  where  the  gains  are  increased  during  maneuvers. 

Further  research  is  needed  to  develop  improved  maneuver  response  procedures  for  the 
two-stage  estimator.  Also,  further  research  is  needed  to  compare  the  tracking  performances  of 
the  fixed-gain,  two-stage  estimators  with  other  simple,  adaptive  tracking  algorithms.  When 
using  the  two-stage  estimator,  the  kinematically  constrained  second  stage  can  be  paralleled 
with  the  standard  second  stage  so  that  constant  speed  and  variable  speed  targets  can  be 
tracked  by  selecting  the  more  accurate  second  stage.  However,  research  is  needed  to  develop 
procedures  for  selecting  the  more  accurate  second  stage. 


7-1 


NSWCDD/TR-92/297 


REFERENCES 


1.  Kalata,  P.R.,  “The  Tracking  Index:  A  Generalized  Parameter  for  a  —  0  and  a  —  0  —  7 
Target  Trackers,”  IEEE  Trans.  Aero.  Elect.  Syst.,  Vol.  AES-20,  March  1984,  pp.  1 74- 
182. 

2.  Cantrell,  B.H.,  Adaptive  Tracking  Algorithms,  NRL  Report,  Washington,  DC,  April 
1975. 

3.  Blackman,  S.S.,  Multiple-Target  Tracking  with  Radar  Applications,  ARTECH  House, 
Inc.,  Norwood,  MA,  1986. 

4.  Alouani,  A.T.,  Xia,  P.,  Rice,  T.R.,  and  Blair,  W.D.,  “A  Two-Stage  Kalman  Estimator 
for  State  Estimation  in  the  Presence  of  Random  Bias  and  Tracking  Maneuvering 
Targets,”  Proc.  of  30th  IEEE  Conf.  on  Dec.  and  Cont.,  Brighton,  UK,  Decemeber 
1991,  pp.  2059-2062. 

5.  Alouani,  A.T.,  Xia,  P.,  Rice,  T.R.,  and  Blair,  W.D.,  “A  Two-Stage  Kalman  Estimator 
for  Tracking  Maneuvering  Targets,”  Proc.  of  IEEE  1991  Inter.  Conf.  on  Syst.,  Man, 
Cyber.,  Charlottesville,  VA,  October  1991,  pp.  761-766. 

6.  Bogler,  P.L.,  “Tracking  a  Maneuvering  Target  Using  Input  Estimation,”  IEEE  Trans. 
Aero.  Elect.  Syst.,  Vol.  AES-24,  May  1987,  pp.  293-310. 

7.  Chan,  Y.T.,  Hu,  A.G.C.,  and  Plant,  J.B.,  “A  Kalman  Filter  Based  Tracking  Scheme 
with  Input  Estimation,”  IEEE  Trans.  Aero.  Elect.  Syst.,  Vol.  AES-15,  March  1979,  pp. 
237-244. 

8.  Bar-Shalom,  Y.  and  Fortmann,  T.E.,  Tracking  and  Data  Association,  Academic  Press, 
Inc.,  Orlando,  FL,  1988. 

9.  Alouani,  A.T.,  and  Blair,  W.D.,  “Use  of  a  Kinematic  Constraint  in  Tracking  Constant 
Speed,  Maneuvering  Targets,”  Proc.  of  30th  IEEE  Conf.  on  Dec.  and  Cont.,  Brighton, 
UK,  December  1991,  pp.  2055-2058. 

10.  Jazwinski,  A.  Stochastic  Processes  and  Filtering  Theory,  Academic  Press,  Orlando, 
FL,  1970. 

11.  Solomon,  D.L.,  “Covariance  Matrix  for  a  —  /3  —  7  Filtering,”  IEEE  Trans.  Aero.  Elect. 
Syst.,  Vol.  AES-21,  May  1988,  pp.  157-159. 

12.  Friedland,  B., “Treatment  of  Bias  in  Recursive  Filtering,”  IEEE  Trans.  Auto.  Cont., 
Vol.  AC-14,  August  1969,  pp.  359-366. 

13.  Alouani,  A.T.,  Rice,  T.R.,  and  Blair,  W.D.,  “A  Two-Stage  Filter  for  State  Estimation 
in  the  Presence  of  Dynamical  Stochastic  Bias,”  Proc.  of  1992  American  Control 
Conference,  Chicago,  IL,  June  1992. 


8-1 


NSWCDD/TR-92/297 


APPENDIX  A 

DERIVATIONS  FOR  ALPHA-BETA  FILTER 
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A  Kalman  filter  is  often  employed  to  filter  the  position  measurements  for  estimating  the 
position,  velocity,  and/or  acceleration  of  a  target.  When  the  target  motion  and  measurement 
models  are  linear  and  the  measurement  and  motion  modeling  error  processes  are  Gaussian, 
the  Kalman  filter  provides  the  minimum  mean-square  error  estimate  of  the  target  state. 
When  the  target  motion  and  measurement  models  are  linear,  but  the  noise  processes  are  not 
Gaussian,  the  Kalman  filter  is  the  best  linear  estimator  of  the  target  state  in  the  mean-square 
error  sense.  The  dynamics  model  commonly  assumed  for  a  target  in  track  is  given  by 

=  FkXk  +  Gkwk  (A.l) 

where  to*  ~  N( 0,  Qk)  is  the  process  noise  and  Fk  defines  a  linear  constraint  on  the  dynamics. 
The  target  state  vector  Xk  contains  the  position,  velocity,  and  acceleration  of  the  target  at 
time  k  as  well  as  other  variables  used  to  model  the  time-varying  acceleration.  The  linear 
measurement  model  is  given  by 

Yk  =  HkXk  +  nk  (A.  2) 

where  Yk  is  usually  the  target  position  measurement  and  nk  ~  N(0,  Rk).  The  Kalman 
filtering  equations  associated  with  the  state  model  in  Eq.  (A.l)  and  the  measurement  model 
in  Eq.  (A. 2)  are  given  by  the  following  equations. 

Time  Update: 


(^-3) 

=  Fk-iPk-i\k-\Fk-i  +  Gjfc-iQ*-iG*_i  (A.4) 

Measurement  Update: 

Kk  =  +  /?*]-'  (A.  5) 

%k\k  =  Xk\k-l  +  (A.6) 

Pk\k  =  K-  KtHk\Pt\k- >  (A.7) 


where  Xk  ~  N(Xk\k ,  Pk\k)  with  A"*|*  and  P*|*  denoting  the  mean  and  error  covariance  of  the 
state  estimate,  respectively.  The  subscript  notation  (A:|j)  denotes  the  state  estimate  for  time 
k  when  given  measurements  through  time  j,  and  Kk  denotes  the  Kalman  gain.  Using  the 
matrix  inversion  lemma  of  [A-l]  and  Eqs.  (A. 5)  and  (A.7),  an  alternate  form  of  the  Kalman 
gain  is  given  by 

Kt  =  Pk\k-iHk\HkPk\k-kHl  +  ftj1]-1 

=  [I-Pk\k-kHlRZ'Hk]-'Pk\k-kHlR~t' 
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=  1/  -  +  R;')-'Hk]Pk\k-iHT flj1 

=  *VtfV  (-4.8) 


The  steady-state  form  of  the  Kalman  filter  is  often  used  in  order  to  reduce  the 
computations  required  to  maintain  each  track.  In  steady-state,  and 

^fc+i|*  =  F**|*-l,  and  Kk  =  K]c—\.  For  a  Kalman  filter  to  achieve  these  steady-state 
conditions,  the  error  processes,  wk  and  nk,  must  have  stationary  statistics  and  the  data 
rate  must  be  constant.  When  the  noise  processes  are  not  stationary  or  the  data  rate  is  not 
constant,  a  filter  using  the  steady-state  gains  will  provide  suboptimal  estimates.  The  a,  /3 
filter  is  the  steady-state  Kalman  filter  for  tracking  nearly  constant  velocity  targets. 


The  a,  f3  filter  is  a  single  coordinate  filter  that  is  based  on  the  assumption  that  the 
target  is  moving  with  constant  velocity  plus  zero-mean,  white  Gaussian  acceleration  errors. 
Given  this  assumption,  the  filter  gains  a  and  /?  are  chosen  as  the  steady-state  Kalman  gains 
that  minimize  the  mean-square  error  in  the  position  and  velocity  estimates.  For  the  a,  /? 
filter, 


**  =  [xk 


Hk  =  [  1  0] 

Rk  =  *1 

Qk  = 


The  a,  0  gains  are  determined  as  in  [A-2]  by  solving  the  simultaneous  equations 

a2  d2 

T'  rp4aw  _  P 

(1  -  O) 

0  =  2(2  -  a)  -  4yA  -  a 


(21.9) 

(-4.10) 

(-4.H) 

(-4.12) 

(-4.13) 

(-4.14) 

(-4.15) 

(-4-16) 

(-4.17) 


where  T  is  the  tracking  index  of  [A-2],  and  o\.  is  the  variance  of  the  acceleration  modeling 
error. 


STEADY-STATE  ERROR  COVARIANCE  AND  GAINS 
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Let  the  steady-state  error  covariance  matrix  of  the  filtered  estimates  for  the  o,  /?  filter 
be  denoted  as 


pk+i\k+i  =  Pk\k  =  P  = 


P 11  P\2 


Pl2  P22 


(A18) 


Using  Eqs.  (A.8),  (A. 12),  and  (A. 13)  gives  the  steady-state  gain 


K  =  PHl<r~2  = 


Thus  using  Eq.  (A. 15)  gives 


M-19) 


Pll  =  acrv 
0  2 

Pi  2  = 


(A.20) 

(A.21) 


Inserting  Eq.  (A.4)  into  Eq.  (A. 7)  and  setting  Pk\k  =  =  P  for  steady-state  gives 


[/  -  KHk)~lP  =  Fk_xPF{_ j  +  Gk-XGTk_xa2 


(A.  22) 


Then 


r  1  O' 

[I-KHk]~l  =  ~  p 

l-a  -  l-o 


(A. 23) 


l l-KHk]-xP  =  TJL-  o  R2 

1  *J  1  -  O  P  P ,  n  \P22 

T  r+('-a)^ 


FkPFl  = 


Pll  +  2Tpi2  +  T2P22  Pl2  +  Tp22 
P\2  +  Tp22  P22 


(A.  24) 


(A. 25) 


'  r£\  J'i  ' 

ri  riT  2  _  2  4  2 

LrkLrk<7w  —  (Tw  » 

,T  H 

Equating  the  (2,2)  elements  of  Eq.  (A. 22)  gives 

ff2  _  T^crJ 

1  -  O  ~  <7* 


ui  _  r>2 


(/1.26) 


(A.27) 
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Equating  the  (1,2)  elements  of  Eq.  (A. 22)  and  using  Eq.  (A.27)  to  eliminate  <7^  gives 


P22 


/?(2a  -  /?)  2 

(1  -a)T2°v 


(A. 28) 


Equating  the  (1,1)  elements  of  Eq.  (A.22)  and  using  Eqs.  (A. 20)  through  (A. 21)  and  Eqs. 
(A.27)  through  (A.28)  gives 

P  =  2(2  -  a)  -  4v/l  -  a  (A. 29) 


Eqs.  (A.27)  and  (A.30)  give  the  steady-state  gain  relationships  in  Eqs.  (A. 16)  and  (A. 17). 
The  steady-state  error  covariance  in  Eq.  (A.  18)  is  given  by  [1,8]  as 


^*|*  = 


a 


l 

T 


t  ^2a  ~  A 

T  2(1  -a)T2 


(A.30) 


MEASUREMENT  VARIANCE  REDUCTION  MATRIX 


The  variance  reduction  between  position  and  velocity  estimates  and  the  measurements 
are  often  considered  in  the  design  and  analysis  of  a,  0  filters.  The  variance  reduction 
matrix  for  the  a,  ft  filter  is  derived  by  considering  the  filter  as  a  linear  system  with  a  white 
noise  input.  The  input-output  relationships  between  the  measurements  V*  and  Xk\k  can  be 
expressed  as  a  linear  system  that  is  given  by 


where 


Xk\k  =  FX  +  GkYk 


(A.31) 


ri  -  a 


(l-a)T- 

(1-/3) 


(A.  32) 

(A.  33) 


Using  Eq.  (A.31)  provides  the  error  covariance  of  X that  results  from  white  noise 
measurement  errors.  That  error  covariance  =  5jt  is  given  by 

Sk  =  F  Sk.1FT  +  G  GTo\  (AM) 
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where  a2  is  the  variance  of  the  input  V*.  Since  S*  =  S*_i  =  Sap  in  steady-state  conditions, 
Eq.  (A. 34)  can  be  used  to  solve  for  elements  of  Sap  in  terms  of  the  filter  gains,  measurement 
period,  and  the  measurement  error  variance.  Eq.  (A. 34)  can  be  rewritten  as 

F~lSa0  =  Sap~FT  +  F~XG  GTa\  (A. 35) 


where 


1 

1  —  Q 


[1-/? 


L  T 


-(l-a)T' 

(1-a) 


- 1 - T 

F  G  G  <j 


2  _ 
v 


1  —  a 


(a  —  0)a 

a.0 


T 


t 

T 2 


Let 


r 


s2i 

S22 


Then,  equating  the  (1,1),  (2,1),  and  (2,2)  elements  of  Eq.  (A. 35)  gives 


[(2  -  a)  -  0)sn  -  (2  -  a)(l  -  a)Tsx2  =  (a  -  0)a<r2v 
fisn  +  a(l  -  a)Ts]2  -  (1  -  a)2T2s22  =  = 

(2  -  a)si2  +  (1  -  a)Ts22  =  =  ~- 

Multiplying  Eq.  (A. 41)  by  (1  —  a)T  and  adding  it  to  Eq.  (A.40)  gives 

T 

sn  =  -2(1  -  a)—s\2  +  <x2 

Inserting  Eq.  (A.42)  into  Eq.  (A. 39)  gives 

_  (2c*  ~  0)0  2 

512  a[4  -  2a  -  p\T°v 

Inserting  Eq.  (A. 43)  into  Eq.  (A.42)  gives 

2a2  +  0(2  —  3a)  2 
SU  ~  a [4  -2 a-0] 

Inserting  Eq.  (A. 43)  into  Eq.  (A. 41)  gives 

202  2 

322  ~  a[A-2Q-0}T*<Tv 


(A.36) 

(A.37) 

(A. 38) 

(A. 39) 
(A.40) 

(A. 41) 

(A.42) 

(A.43) 

(A. 44) 


(A.45) 
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Eqs.  (A.43)  through  (A.45)  with  Eq.  (A. 38)  gives 


where  d\  =  4—2 a—0.  The  variance  reduction  ratios  of  the  filter  are  given  by  Eq.  (A.46)  when 
<r2  =  1  with  the  (1,1)  and  (2,2)  elements  of  Sap  denoting  the  position  variance  reduction 
ratio  and  velocity  variance  reduction  ratio,  respectively  [A-3]. 

INITIALIZATION  GAINS 

For  tracking  systems  with  a  uniform  data  rate  and  stationary  measurement  noise, 
nonmanem  --:ring  targets  can  be  accurately  tracked  with  a  steady-state  Kalman  filter. 
However,  between  filter  initialization  and  steady-state  conditions,  the  Kalman  gain  and 
state  error  covariances  are  transient.  If  steady-state  gains  are  used  from  initialization,  the 
settling  time  of  the  state  estimates  may  be  extended  significantly.  While  the  initial  gains  can 
be  approximated  as  decaying  exponentials  as  suggested  in  [A-2],  identifying  the  constants 
for  the  exponential  can  be  quite  cumbersome.  The  purpose  of  this  section  is  to  present  a 
simple  gain  scheduling  procedure  for  initializing  a,  3  filters. 

The  gain  scheduling  procedures  are  developed  by  using  the  motion  and  measurement 
models  to  formulate  a  batch  least-squares  estimation  problem  for  an  arbitrary  number  of 
measurements.  An  analytical  form  is  then  obtained  for  the  resulting  error  covariance  that  is 
used  in  conjunction  with  Eq.  (A. 8)  to  obtain  an  analytical  form  for  the  Kalman  gain. 

For  linear  least-squares  estimation,  an  equation  formulating  the  measurement  vector 
Z  as  a  linear  function  of  the  parameter  vector  X  to  be  estimated  is  given  by 

Z  =  WX  +  V  (A. 47) 

where  E\V\  =  0  and  E[VVT ]  =  cr2Iy  for  N  measurements  with  Iy  denoting  the  N  x  N 
identity  matrix.  The  least-squares  estimate  X  is  given  in  [A-4]  as 

X  =  (WTW)-'WTZ  (A. 48) 

with  error  covariance 

PN  =  COV[A]  =  *2(WTW)~l  (A. 49) 


c  _  fL 

Safi  ~  ad i 


2a2  +  0(2  -3a)  |(2a  -  fi) 

3  2 32 

£(2a  -3)  YZ 
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A  current  state  estimate  of  X  denoted  by  Xo  =  [xo  xo]  can  be  obtained  from  the 
current  measurement  plus  the  N  previous  measurements  as 


where 


Zn  =  WffX  o  +  Vy 


Zn  =  [y-N  y-N+i 
Vn  =  [v-N  V-n+i  ...  u_!  u0]J 

w„-<  1  1 


(A.50) 

yof 

(A. 51) 

”o]r 

(A. 52) 

1  l' 
-T  0 

T 

(A. 53) 

with  E[Vn]  =  0  and  E[VffV^]  =  The  error  covariance  of  the  least-squares  estimate 

Xo  is  given  by 

COV[X0]  =  Pjv  =  <rl{WlWN)-x 


First, 


(W%W„)  = 


N 


n  + 1  r£(- n  -i-  i) 


iV 


i=0 

N 


r£(-JV  +  >)  X2£(JV-,')! 


i= 0 


i=0 


W+l  -T'Ej 

j= o 

N  N 

-TY.j  T^i2 

7=0  7=0 


Evaluating  the  finite  summations  of  Eq.  (A. 55)  gives 

1 


N 


{WffWN)  =  (JV  +  1) 


T 

■N2 


—N—  N{2N  +  1) 


T 2 


Then 


COV[Xo]  =  P*  = 


2ff2 


(A  +  l)(N  +  2) 


2A  +  1 

3 

T 


3 

T 

6 


(A  -  1)T2 


(A. 54) 


(A.55) 


(A. 56) 


(A. 57) 
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Let  the  Kalman  gain  be  denoted  by 


Kk 


(A.58) 


Letting  Pjfcj*  =  Pk  in  Eq.  (A. 8)  provides  a  simple  gain  scheduling  procedure  for  a  and  /? 
during  initialization  that  is  given  by 

°t  =  maX<(t+W  +  2)'a>  (A59) 

A  =  max{(tTWT2)'w  ('4-60) 

with  Xo|-l  =  [0  0]  .  Note  that  if  Yq/T  can  be  extremely  large  relative  to  dynamic  range 
of  the  computer,  the  first  two  filter  iterations  should  be  computed  analytically  to  use  a 
two-point  initialization  procedure. 
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APPENDIX  B 

DERIVATIONS  FOR  ALPHA-BETA-GAMMA  FILTER 


B-l 


NSWCDD/TR-92/297 


A  Kalman  filter  is  often  emplc ^ed  to  filter  the  position  measurements  for  estimating  the 
position,  velocity,  and/or  acceleration  of  a  target.  When  the  target  motion  and  measurement 
models  are  linear  and  the  measurement  and  motion  modeling  error  processes  are  Gaussian, 
the  Kalman  filter  provides  the  minimum  mean-square  error  estimate  of  the  target  state. 
When  the  target  motion  and  measurement  models  are  linear,  but  the  noise  processes  are  not 
Gaussian,  the  Kalman  filter  is  the  best  linear  estimator  of  the  target  state  in  the  mean-square 
error  sense.  The  dynamics  model  commonly  assumed  for  a  target  in  track  is  given  by 

Xk+i  =  FkXk  +  Gkwk  (B.  1) 

where  wk  ~  iV(0,  Qk)  is  the  process  noise  and  Fk  defines  a  linear  constraint  on  the  dynamics. 
The  target  state  vector  Xk  contains  the  position,  velocity,  and  acceleration  of  the  target  at 
time  k ,  as  well  as  other  variables  used  to  model  the  time-varying  acceleration.  The  linear 
measurement  model  is  given  by 

Yk  =  HkXk  +  nk  (B.  2) 

where  Yk  is  usually  the  target  position  measurement  and  nk  ~  N( 0,  Rk).  The  Kalman 
filtering  equations  associated  with  the  state  model  in  Eq.  (B.l)  and  the  measurement  model 
in  Eq.  (B.2)  are  given  by  the  following  equations. 

Time  Update: 


=  Fk-iXk-\\k~i 

(B.  3) 

pk\k-\  =  Fk-iPk-\\k-iFk-i  +  Gk-iQk-iGl^ 

(B.4) 

Measurement  Update: 

«k  =  Pkik-iHl\HkPk\k-iHl  +  Rk)-' 

(B.5) 

Rk\k  =  +  Kk[Yk  -  HkRk\k-l] 

(B.6) 

Pk\k  =  V-KkHk]Pk\k-l 

(B.7) 

where  Xk  ~  N(Xk\k>  Fk\k)  with  and  denoting  the  mean  and  error  covariance  of  the 
state  estimate,  respectively.  The  subscript  notation  {k\j)  denotes  the  state  estimate  for  time 
k  when  given  measurements  through  time  j,  and  Kk  denotes  the  Kalman  gain.  Using  the 
matrix  inversion  lemma  of  [B-l]  and  Eqs.  (B.5)  and  (B.7),  an  alternate  form  of  the  Kalman 
gain  is  given  by 

Kk  =  Pk\k-,Hl\HtPk^Hl  +  V]"1 
=  [I  ~  Pk\k-xHl Rl' Ht\-' Pt^Hl Rl' 
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=  [/-  Pt\k-lHl(HkPk\t-lHl  + 

=  %«tTV  (b.  8) 


The  steady-state  form  of  the  Kalman  filter  is  often  used  in  order  to  reduce  the 
computations  required  to  maintain  each  track.  In  steady-state,  1  |Jt— i »  and 

A+i|*  =  i »  and  Kk  =  Kk~ i-  For  a  Kalman  filter  to  achieve  these  steady-state 
conditions,  the  error  processes,  w *  and  n*,  must  have  stationary  statistics  and  the  data 
rate  must  be  constant.  When  the  noise  processes  are  not  stationary  or  the  data  rate  is  not 
constant,  a  filter  using  the  steady-state  gains  will  provide  suboptimal  estimates.  The  a,/3, 7 
filter  is  the  steady-state  Kalman  filter  for  tracking  nearly  constant  acceleration  targets. 


The  a,  fl,  7  filter  is  a  single  coordinate  filter  that  is  based  on  the  assumption  that  the 
target  is  moving  with  constant  acceleration  plus  zero-mean,  white  Gaussian  acceleration 
errors.  Given  this  assumption,  the  a,0,-y  filter  gains  are  chosen  as  the  steady-state 
Kalman  gains  that  minimize  the  mean-square  error  in  the  position,  velocity,  and  acceleration 
estimates.  For  the  a,  /?,  7  filter, 


Xk  =  [z* 
Fk  = 


Gk  = 

Hk  =  [  1 
Rk  =  <*l 
Qk  = 

Kk  = 


rp2 

—  T 

l  2 


*k  Xk]2 

T  0.5  T2 
1  T 

0  1 

T 
I 

1 


0  0] 


a  — 


i 

T 


rp2 


^T 


The  a,/?, 7  gains  are  determined  as  in  [B-2]  by  solving  the  simultaneous  equations 

_2  *> 

< t r 


V 

7  = 


(1-a) 

2(2  —  q)  —  4\/l  —  a 

t 

2a 


where  T  = 


T2a,, 


is  the  tracking  index  of  [B-2]. 

B-4 


(5.9) 

(5.10) 

(5.11) 

(5.12) 

(5.13) 

(5.14) 

(5.15) 

(5.16) 

(5.17) 

(5.18) 
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STEADY-STATE  ERROR  COVARIANCE  AND  GAINS 

Let  the  steady-state  error  covariance  matrix  of  the  filtered  estimates  for  the  a,/?, 7 
filter  be  denoted  as 


P11  P12  P 13 
P  =  Pl2  P22  P23 


(B.  19) 


.  Pl3  P23  P33. 


Using  Eq.  (B.8),  (B.12),  and  (B.13)  gives  the  steady-state  gain 


k  =  phTo:2  = 


(B.20) 


Thus  using  Eq.  (B.15)  gives 


Pll  =  ourv 

0  2 

P 12  =  j,°v 

7  2 

Pl3  rjl2 


(B.  21) 
(B.  22) 
(B.  23) 


Inserting  Eq.  (B.4)  into  Eq.  (B.7)  and  setting  Pk\k  =  Pjt_i|fc_i  =  P  for  steady-state 


\I-KHk\-'P  =  Fj.jPff.,  +Gt-lGTk_l*l 


(B- 24) 


Then 


'1  0  O' 

[/ - k//*]-1  =  |  1-0  0 

1  —  a  1 

Y2  0  l~Q 


(£.25) 


0  0 


P22  01 


v-K’ur'p-r^  t 


^  v  t; 

7  07,  n  \P23  72  xP33 

T2  jS+t1  a)a2  r4+(1  ^<72 


(£.26) 
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FkPF{  = 


j ’4 

pil  +  2Tpi2  +  T2(p\3  +  P22)  +  T*P23  +  — P33 

rp  3 

+  YP33 


GkGja2w  =  cri 


P12  +  T(p\z  +  P22)  + 

3  T2 
2 

„  T2 

P13  +  T P23  + 

‘  rp4 

rpZ 

T2 

T 

T 

2 

rp3 

T 

j'2 

T 

rp2 

T 

1 

P22  +  2Tp23  +  T2P33  ... 

P23  +  ?P33  P33 

(B.27) 


(B.28) 


Equating  the  (3,3)  elements  of  Eq.  (B.24)  gives 


?  T4<t2 


1  —  a  a2 


—  —  p2 


(B.29) 


Equating  the  (2,3)  elements  of  Eq.  (B.24)  and  using  Eq.  (B.29)  to  eliminate  gives 


_  7(^  ~  7)  2 
m  ~  (l -a )T*  v 


(B.  30) 


Equating  the  (2,2)  elements  of  Eq.  (B.24)  and  using  Eq.  (B.29)  to  eliminate  cr^  gives 


_  -  JULZ-OLJ 

m  2(1 -a  )T*V 


(B.31) 


Equating  the  (3,1)  elements  of  Eq.  (B.24)  and  using  Eqs.  (B.20),  (B.29),  (B.30),  and  (B.31) 
gives 

32 

(B.  32) 

Equating  the  (2,3)  elements  of  Eq.  (B.24)  and  using  Eqs.  (B.20),  (B.29),  and  (B.31)  gives 

4a/3  4-  7 (/?  —  2a  —  4)  2 


P22  = 


4(1  -  a)T2 


(B.  33) 


Equating  the  (1,1)  elements  of  Eq.  (B.24)  and  using  Eqs.  (B.21)  through  (B.23)  and  Eqs. 
(B.29)  through  (B.33)  gives 

p  =  2(2  -  a)  -  Wl-a  (B. 34) 


B-6 


NSWCDD/TR-92/297 


Eqs.  (B.29),  (B.32),  and  (B.34)  give  the  steady-state  gain  relationships  in  Eqs.  (B.16) 
through  (B.18).  The  steady-state  error  covariance  in  Eq.  (B.19)  is  given  by 

0  7 

°  T  T 2 

p  2  P_  4a/3  +  7 (0  -  2a  -  4)  0(0  -  7) 

”7’  4(1  —  a)T2  2(1  —  a)T3 

_7_  fl(fl  ~  7)  j(0_  ~  7) 

.  T2  2(1  -  a)T3  (1  -  a)T4 

INITIALIZATION  GAINS 

For  tracking  systems  with  a  uniform  data  rate  and  stationary  measurement  noise, 
nonmaneuvering  targets  can  be  accurately  tracked  with  a  steady-state  Kalman  filter. 
However,  between  filter  initialization  and  steady-state  conditions,  the  Kalman  gain  and 
state  error  covariances  are  transient.  If  steady-state  gains  are  used  from  initialization,  the 
settling  time  of  the  state  estimates  can  extended  significantly.  While  the  initial  gains  can  be 
approximated  as  decaying  exponentials  as  suggested  in  [B-2],  identifying  the  constants  for 
the  exponential  can  be  quite  cumbersome.  The  purpose  of  this  section  is  to  present  some 
simple  gain  scheduling  procedures  for  initializing  a,  0, 7  filters. 

The  gain  scheduling  procedures  are  developed  by  using  the  motion  and  measurement 
models  to  formulate  a  batch  least-squares  estimation  problem  for  an  arbitrary  number  of 
measurements.  An  analytical  form  is  then  obtained  for  the  resulting  error  covariance  that  is 
used  in  conjunction  with  Eq.  (B.8)  to  obtain  an  analytical  form  for  the  Kalman  gain. 

For  linear  least-squares  estimation,  an  equation  formulating  the  measurement  vector 
Z  as  a  linear  function  of  the  parameter  vector  X  to  be  estimated  is  given  by 

Z  =  WX  -f  V  (B. 36) 

where  E[V\  =  0  and  E[VVT]  =  <r2/jv  for  N  measurements  with  /jv  denoting  the  N  x  N 

>■*« 

identity  matrix.  The  least-squares  estimate  X  is  given  in  [B-3]  as 

X  =  C WTW)~lWTZ  (B.  37) 

with  error  covariance 

PN  =  COV[X]  =  <r2(WTW)~1  (B.  38) 
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A  current  state  estimate  of  X0  =  [xo  xo  x0]  can  be  obtained  from  the  current 
measurement  plus  the  N  previous  measurements  as 


Zff  =  WnXq  +  Vn 


(5.  39) 


where 


T 

Zn  =  [y-N  y-N+ 1  •••  y-\  yo] 

T 

Vn  =  [v~N  V-JV+1  •••  *>-l  ^o] 

1  1 

WN  =  -NT  {-N  +  \)T 

Lo.5(at)2  o.5(-n  +  i)2t2  . 


i  i 

-T  0 

o.sr2  o  J 


(5.40) 

(5.41) 

(5.42) 


with  E[Vn]  =  0  and  E[V ’nV£]  =  The  covariance  of  the  least-squares  estimate  Xq  is 

given  by 

COV|Xol  =  Py  =  °l(w];wK)-' 


(B.43) 


First, 


(W$WN)  = 


N 


N- f  1 


T'El-N  +  i)  Y  £(*-■)’ 


1=0 

N 


i=0 

N 


N  N  t3  N 

t£(-jv  +  ;)  r2£(jv->)2  —  £(-iv  +  >) 


.=0 

N 


i=0 

N 


tEh+')’ 


i=0 

JV  +  1 

N 

~TZ’ 

1=0 


i=0 


i- 0 
JV 


T 2 


N 


N 

-TZ> 

j=0 

N 

t2Z>2 

j=0 
_  j>3  N 


T2  A 

^Z? 


-T3 


j= o 

N 


Zr 


Z I2  ^Zi 


J-0 


j= 0 


>=0 
T4  ^ 

i=0 


(5.44) 
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Evaluating  the  finite  summations  of  Eq.  (B.44)  gives 
(W%WN)  =  (N  + 1). 

1  -jv|  W(2JV  +  1)^ 

-nZ  N(2N  +  1)~  -N*(N  +  1)j- 

JV(22V  +  1)^  -JV2(JV+1)^-  N(2N  +  l)(3iV2  +  3iV  —  l)~: 

-  VZ  O  1  Z\) 


(£.45) 


Then 

( Wj,WN)PN  =  oih 

Let  the  error  covariance  be  denoted  as 


'an 

ai2 

O13 " 

Pn  = 

ai2 

<122 

023 

.013 

a2s 

033. 

(£.46) 


(£.47) 


Equating  the  elements  of  Eq.  (B.46)  gives  the  following  six  equations  t.>  solve  for  P^: 


N°U  2ai2  +  (2AT  +  l)12ai3  +  j)  (5<48) 

-  ^-<*22  +  (2 N  +  1)— 1 023  =  o  (£.49) 

-fia  13  -  2-023  +  (2JV  +  1)— 033  =  0  (£.50) 

-f  ai2  +  (2  N  +  1)^  a22  -  N(N  +  l)ja23  =  ^  yy  (£.51) 

—  2-013  +  (2iV  +  1)— a23  —  N(N  +  1)— 033  =  0  (£.52) 

(2JV  +  l)~oi3  -  N(N  +  1  )y  023  +  (2 N  +  l)(3iV2  +  3  N-  1  )^a33  =  N{f+l)  (B.53) 


Solving  Eqs.  (B.48)  through  (B.53)  gives 

COV[X0]  =  Pn  = 

3N2  +  3N  +  2 

3  a2  6(2iV+l) 

(N  +  \)(N  +  2)(N  3)  T 

20 

j>2 


6(2iV  +  l) 

T 

4(2AT+l)(87V-3) 

{N  -  \)NT2 

120 

(iV  -  i)r3 


20 

T2 


120 

(N-  \)T3 
240 

(iv-  i)ivr4 


(£.54) 
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Let  the  Kalman  gain  be  denoted  by 


Kk  = 


0k  Ik 

ak  rp  p2 


iT 


(5.55) 


Letting  Pk\k  =  Pk  in  Eq.  (B.8)  gives  a  simple  gain  scheduling  procedure  for  ak ,  /?*  and  7* 
during  initialization  that  is  given  by 


ak  =  max{ 
0k  =  max{ 
7 k  =  max{ 


3(3  k2  +  3ifc  +  2) 

<fc  +  l)(fc  +  2)(ib  +  3),a) 
18(2* +1)  « 
(ifc  +  l)(ifc  +  2)(ifc  +  3)’P; 
60 

(ifc-f  l)(fc  +  2)(Jfc  +  3),7) 


(5.56) 

(5.57) 

(5.58) 


T  o 

with  X0)_i  =  [0  0  0]  .  Note  that  if  Yq^/T2  can  be  extremely  large  relative  to  the 
dynamic  range  of  the  computer,  the  first  three  iterations  of  the  filter  should  be  computed 
analytically  to  use  a  three-point  initialization  procedure. 
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